
    \h8K                         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  S rS rS rS	 rg
)    )cosMatrixsinzerostanpisymbols)simplify)trigsimp)solve)
crossdotdynamicsymbolsfind_dynamicsymbolsKanesMethodinertiainertia_of_point_massPointReferenceFrame	RigidBodyc                     [        S5      u  pp#pE[        S5      u  pgn[        S5      n	U	 V
s/ s H  oR                  U 5      PM     nn
[        S5      nU Vs/ s H  oR                  U 5      PM     nn[        [	        US/[        U5      -  5      5      n[        S5      n[        [	        US/[        U5      -  5      5      n[        S5      nUR                  SS	U	S
   UR                  /5      nUR                  SS	U	S   UR                  /5      nUR                  SS	U	S   UR                  /5      nUR                  UUS
   UR                  -  US   UR                  -  -   US   UR                  -  -   5        UR                  UUR                  U5      R                  U U5      [        UR                  U5      UR                  U5      5      -   5        [        S5      nUR!                  UUS
   UR                  -  US   UR                  -  -   US   UR                  -  -   5        UR#                  SU	S   UR                  -  U[%        U	S   5      -  UR                  -  -   5      nUR!                  UUS   UR                  -  US   UR                  -  -   US   UR                  -  -   5        UR'                  UUR)                  U5      R                  U U5      [        UR                  U5      UR)                  U5      5      -   5        US
   UR                  -  US   UR                  -  -   US   UR                  -  -   nUR+                  U5      R                  U U5      [        UR                  U5      UR+                  U5      5      -   n[-        U Vs/ s H!  n[/        UUR                  U5      -
  U5      PM#     sn[/        UUR)                  U5      -
  UR                  5      /-   5      n[1        UU5      n[1        UR3                  US   S
US   S
05      U5      nUR5                  US   S
US   S
05        UU-    Vs/ s H$  nUR                  U5      R                  UU5      PM&     nnUU-    Vs/ s H$  nUR)                  U5      R                  UU5      PM&     nnUU-    Vs/ s H$  nUR)                  U5      R                  UU5      PM&     n n[-        [/        U* UR                  -  UR                  5      U	S   -
  /5      n![-        U V"s/ s Hi  n"[/        UR)                  U5      UR)                  U5      [        UR                  U5      UR+                  U5      5      -   -
  U"5      R7                  5       PMk     sn"5      n#[        UR                  U5      UR+                  U5      5      n$U$R                  U U5      [        UR                  U5      U$5      -   n%[-        U V"s/ s H!  n"[/        UR9                  U5      U%-
  U"5      PM#     sn"5      n&[;        SS5      n'[=        S5       H4  n([?        UU-   5       H  u  n)nU#U(   R                  U5      U'U(U)4'   M!     M6     U'S S 2S S24   n*U'S S 2SS24   n+U'S S 2SS 24   n,U*RA                  U,5      n-U+RC                  5       * U--  n.U.S S 2S S24   [-        US S 5      -  n/[        [	        USS  U/5      5      n0X#-  UR                  -  n1UUR                  -  UUR                  -  -   UUR                  -  -   n2[-        [	        UU 5       V3V4s/ s H  u  n3n4[/        U1U35      [/        U2U45      -   PM!     sn4n35      n5U* UR9                  U5      -  n6[E        UXEU5      n7[/        U7URG                  U5      5      [        UR                  U5      [/        U7UR                  U5      5      5      -   * n8[-        [	        UU5       V9V:s/ s H  u  n9n:[/        U6U95      [/        U8U:5      -   PM!     sn:n95      n;U5S S2S S 24   RI                  U5SS 2S S 24   5      U.RJ                  U5SS2S S 24   -  -   n<U;S S2S S 24   RI                  U;SS 2S S 24   5      U.RJ                  U;SS2S S 24   -  -   n=U=R3                  U5      R3                  U05      R3                  U5      R3                  U	S   U* [M        U	S   5      -  05      R7                  5       n>U7U4n?[O        SUUUU?5      n@U@/nAUU14nBUU24nCUBUC/nD[Q        UU	S S US S UU	SS  U!USS  U#US9	nEUERS                  UAUD5      u  nFnGUGR3                  U5      R3                  U05      R3                  U5      R3                  U	S   U* [M        U	S   5      -  05      R7                  5       nHUERU                  5       nI[-        U<5      R7                  5       UFR7                  5       :X  d   e[-        U=R3                  WI5      5      R7                  5       WGR7                  5       :X  d   e[W        [-        U>5      R7                  5       5      RY                  S
S05      [W        WHR7                  5       5      RY                  S
S05      :X  d   e[[        WER\                  5      nJU H  nKUKWJ;  a  M   e   g s  sn
f s  snf s  snf s  snf s  snf s  snf s  sn"f s  sn"f s  sn4n3f s  sn:n9f )Nzt r m g I JzFx Fy Fzzq:4zu:6g        zua:3NAAxisr   B   C   PO         	      disc)q_indu_indkd_eqsq_dependentconfiguration_constraintsu_dependentvelocity_constraintsu_auxiliary)/r	   r   diffdictziplenr   	orientnewzxyset_ang_velset_ang_acc
ang_vel_inr   r   set_vel	locatenewr   set_accvelpos_fromr   r   r   subsupdateexpandaccr   range	enumeraterow_joininvr   
ang_acc_incol_joinTr   r   r   kanes_equationskindiffdictr
   xreplacer   forcing)LtrmgIJFxFyFzqqiqduuiudud_zerouaua_zeror   r   r   r   r   r    w_c_n_qdv_o_n_qduvkindiffsqd_kdsteady_conditionspartial_w_Cpartial_v_Opartial_v_Pf_caif_vv_o_na_o_nf_aM_vijM_v_iM_v_dM_v_aux	M_v_i_auxA_rsu_dep
u_dep_dictF_OF_Ppv_opv_pFr_uR_star_OI_C_OT_star_Cpvpav	Fr_star_uFr_c	Fr_star_cFr_star_steady
iner_tupler&   bodyListF_oF_p	forceListkanefrfrstarfrstar_steadykddsyms_in_forcingqdisL                                                                               `/var/www/auris/envauris/lib/python3.13/site-packages/sympy/physics/mechanics/tests/test_kane2.pytest_aux_depr      s	   " }-A!$JBB& 	uA 	!q''!*qB	!uA 	!q''!*qB	!3rB4B<()G		B3rB4B<()G 	sA	C!A$-A	C!A$-A	C!A$-A MM!QqT!##X!QSS(1Q4834MM!Q\\!_))!Q/1<<?ALLO<= > 	c
AIIaAqssRU133Y&Aqss23	C1accAc!A$iKO34AIIa1accAaDH$qtACCx/0IIaqq!$uQ\\!_aeeAh'GGH !uQSSy2a59$r!uQSSy0Hzz!}!!!Q'%QA*OOHQGQrs8all1o5r:QG8aeeAh.456 7H(BE hmmRUQ1,BCQGbeaA34 :;R@21<<?''A.K@23b&9&B1558==Q'&K923b&9&B1558==Q'&K9 #qbfacc"QqT)*
+C
01301" aeeAh!%%(U1<<?	

1. # #VX&013 4C!,,q/1::a=1EJJq!uQ\\!_e<<E
a8a#aeeAh&+a8
9C 1+C1Xq2v&EArABC1I '  2A2JE1Q3KE!QR%jGw'IYY[=9$DBQBK&2A-'Ec!AB%'(J #acc'C
qss(R!##X
QSS
(C[)+) 8BtT3sD>CTN2)+ ,D r!%%({HAqQEUALLO,a#eQ\\!_*EFG HHK575 DK2sHb)C#,>>57 8I
 A;QRU,tvvQqS!V/DDD"1"a% )))ABE*:;&&9QqS!V,,-I^^G,11*=T#$TT1Q4!C!I*>%?  JVQ1j1DvH c(Cc(CsI 	!BQ%"1habEsabE	
D &&x;LRKK(--j9>>?PQT1Q4!C!I./ 



C$< BIIK///)..%&--/6==?BBB VN+2245>>#wG]))+,55qg>? @ ? *$,,7O/))) e 
"	!J H A993 96+7s<   j/j4(j9	+j>=+k1+kA0k3(k=&k
2&k
c                  ,   [        S5      u  pn[        SSS9u  p4n[        S5      u  pgpn
[        S5      u  ppnnn[        S5      u  nnnnnnnn[        S5      u  nnn[        S5      u  nnn[        S	5      n U R                  S
SU* U R                  /5      n!U!R                  SSU U!R
                  /5      n"U"R                  U UU"R
                  -  UU"R                  -  -   5        U"R                  SSUU"R                  /5      n#U"R                  SSUU"R                  /5      n$U#R                  U"U	U"R                  -  5        U$R                  U"U
U"R                  -  5        [        S5      n%U%R                  U"S5        U%R                  U UU"R                  -  5        U%R                  SUU"R                  -  5      n&U%R                  SUU"R                  -  UU"R
                  -  -
  5      n'U&U'4 H  n(U(R                  U%U U"5        M     U%R                  SUU"R                  -  5      n)U%R                  SUU"R                  -  5      n*U%R                  SU* U"R                  -  5      n+U)U*U+4 H  n(U(R                  U%U U"5        M     U*R                  SU* U"R
                  -  5      n,U+R                  SU* U"R
                  -  5      n-U,R                  U*U U#5        U-R                  U+U U$5        X6-
  XI-
  XZ-
  /n.U,U-4 V(s/ s H(  n([        U(R                  U 5      U"R                  5      PM*     n/n([        U"UUUSUS5      n0[        U#UUU5      n1[        U$UUU5      n2[        SU)U"UU0U)45      n3[        SU*U#UU1U*45      n4[        SU+U$UU2U+45      n5[!        U XU/Xg/U.X/U/U/S9n6U&U* U-  U R
                  -  4U'UU"R
                  -  UU"R                  -  -   UU"R                  -  -   4/n7U3U4U5/n8U6R#                  U8U75      u  n9n:[%        U/X/5      n;['        USU-  US-  -  US-  -  -   SU-  -   UUS-  -  -   SU-  US-  -  -   * UR)                  U5      -  UU-  U-  U-  -
  USU-  -   SU-  US-  -  -   * UR)                  U5      -  UU-  US-  -  -   S/5      n<[+        U:R-                  U;5      R-                  US05      5      R/                  5       R1                  5       nU<U-
  R1                  5       [3        SS5      :X  d   e/ n=[5        U3U4U5/U0U1U2/5       H  u  n>n?U?[7        U>R8                  U>R:                  R=                  U%5      U>R>                  5      -   n@U=RA                  [        SU>R:                  U>R>                  U>R8                  U@U%45      5        M     U6R#                  U=U75      u  nAnB[+        UBR-                  U;5      R-                  US05      5      R/                  5       nU<U-
  R1                  5       [3        SS5      :X  d   eg s  sn(f )Nq1:4r   levelzu1:6u' R, M, g, e, f, thetaa b mA mB IA J K tz	Q1, Q2 Q3IA22 IA23 IA33Fr   axisr   r   r   Dr   S*QA*B*C*B^C^rbArbBrbC)r'   r(   r)   r,   r-   r.   r   r!    )!r   r	   r   r3   r6   r5   r7   r4   r   r:   r;   v2pt_theoryr   r=   r   r   r   rJ   r   r   r/   r   r?   doitrA   r   r1   r   mass
masscenterr>   frameappend)Cq1q2q3q1dq2dq3du1u2u3u4u5u_primeRMrQ   efthetaabmAmBIArS   KrN   Q1Q2Q3IA22IA23IA33r   r   r   r   r   pDpS_starpQppA_starpB_starpC_starpB_hatpC_hatkdevc	inertia_A	inertia_B	inertia_Cr   r   r   kmforcesbodiesr   fr_starvc_mapfr_star_expectedbodies2rbI_starrR   fr2fr_star2sC                                                                      r   test_non_central_inertiar      s
     'JBB"63MCc'/BBB$+,F$G!GaE '(< =Aq"b"aA%JBB/0D$ 	sA	C5&!##/A	C"acc+AMM!RVbf_%	C"acc+A	C"acc+AMM!R!##XMM!R!##X 
sBJJq!JJq"qss(ll4133'G	c1QSS51QSS5=	)Br]	b!Q  ll4133'Gll4133'Gll4!ACC(Gw(	b!Q ) taRV,FtaRV,F
w1%
w1% 8SXsx
(C'-v&6	7&6#aeeAh
&6B	7 2tT1dA6I1a#I1a#I E7ArIw+?
@C
E7ArIw+?
@C
E7ArIw+?
@C	Qrrl2(3"$"$
'B 1QSS!B133ACC"QSS&(@#ABF3_F$$VV4KB2x F 1Q3q!t8AqD= 1Q3&Ag"QT	" #%'WWQZ024Q$r'"*=1R4i1QT!"RWWQZ/"Q$r1u*<	 
 	f%**B73499;BBDA!))+uQ{:;: G3S/Iy)+LM
F*277+-==+A+A"+E+-885 5 	yR]]BHHbgg"#R* 	+	 N &&w7MCv&++RG45::<Aq ((*eAqk999] 
8s   ?/Xc            	      
   [        S5      u  pn[        SSS9u  p4n[        S5      u  pgn[        S5      u  pppn[        S5      u  nnnnnnnn[        S5      u  nnn[        S5      u  nnn[        S	5      nUR                  S
SU* UR                  /5      nUR                  SSU UR
                  /5      n U R                  UUU R
                  -  UU R                  -  -   5        U R                  SSUU R                  /5      n!U R                  SSUU R                  /5      n"[        S5      n#U#R                  U S5        U#R                  UUU R                  -  5        U#R                  SUU R                  -  5      n$U#R                  SUU R                  -  U
U R
                  -  -
  5      n%U#R                  SUU R                  -  5      n&U#R                  SUU R                  -  5      n'U#R                  SU* U R                  -  5      n(U$U%U&U'U(4 H  n)U)R                  U#UU 5        M     U'R                  SU
* U R
                  -  5      n*U(R                  SU
* U R
                  -  5      n+U*R                  U'UU!5        U+R                  U(UU"5        U*U+4 V)s/ s H(  n)[        U)R                  U5      U R                  5      PM*     n,n)U,Xc-
  /-  n,[        U,X4U/5      n-[        U-R!                  5       5       H)  u  n.n/U/R#                  U5      U-U.R#                  U5      '   M+     [%        U UUUSUS5      n0[%        U!UUU5      n1[%        U"UUU5      n2['        SU&U UU0U&45      n3['        SU'U!UU1U'45      n4['        SU(U"UU2U(45      n5[)        UXU/Xg/U,U/S9n6U$U* U-  UR
                  -  4U%UU R
                  -  UU R                  -  -   UU R                  -  -   4/n7U3U4U5/n8[+        UU-  X-  U-  [-        U5      -  [/        U 5      -  -   UX-  [-        U5      -  [-        U 5      -  -   X-  U-  [/        U5      -  UU-  -
  UU
-  -
  /5      n9[+        USU-  US-  -  U
S-  -  -   SU-  -   UUS-  -  -   SU-  US-  -  -   * UR#                  U5      -  UU-  U-  U-  -
  USU-  -   SU-  U
S-  -  -   * UR#                  U5      -  UU-  US-  -  -   S/5      n:U6R1                  U8U75      u  n;n<U;R3                  5       U9R3                  5       :X  d   eU:[5        U<5      -
  R3                  5       [7        SS5      :X  d   eg s  sn)f )Nr   r   r   zu1:4r   r   r   zQ1 Q2 Q3r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   )r)   r.   r   r!   )r   r	   r   r3   r6   r5   r7   r4   r   r:   r;   r   r   r=   r   listitemsr/   r   r   r   r   r   r   rJ   rA   r   r   )=r   r   r   r   r   r   r   r   r   r   r   r   rQ   r   r   r   r   r   r   r   r   rS   r   rN   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   kde_mapkvr   r   r   r   r   r   r   r   r   fr_expectedr   r   r   s=                                                                r   test_sub_qdotr   *  sV   
  'JBB"63MCc'JBB$+,F$G!GaE '(< =Aq"b"aA/0D$$JBB 	sA	C5&!##/A	C"acc+AMM!RVbf_%	C"acc+A	C"acc+A 
sBJJq!JJq"qss(ll4133'G	c1QSS51QSS5=	)Bll4133'Gll4133'Gll4!ACC(Gr7GW5	b!Q 6 taRV,FtaRV,F
w1%
w1% )/'7
8'7!3quuQx'7C
8BH:CC#C)GW]]_%1VVAYq	 & 2tT1dA6I1a#I1a#I E7ArIw+?
@C
E7ArIw+?
@C
E7ArIw+?
@C 
Qrhs	MB1QSS!B133ACC"QSS&(@#ABF3_F bD13q5U#CG++SZB''CE#e*r!t#bd*, -K
 1Q3q!t8AqD= 1Q3&Ag"QT	" #%'WWQZ024Q$r'"*=1R4i1QT!"RWWQZ/"Q$r1u*<	  $$VV4KBIIK;--//0/ 1199;uQ{JKJS 9s   /T=c                  B   [        S5      u  pp#pEn[        S5      n[        SSS9n[        S5      n	[        S5      n
U
R                  SSUS	   U
R                  /5      nUR                  S
S[
        S-  US   -
  UR                  /5      nUR                  SSUS   UR                  /5      n[        S5      nUR                  U
S	5        UR                  SUS   U
R                  -  US   U
R                  -  -   5      nUR                  XR                  U5      R                  Xj5      5        UR                  US	5        UR                  SS	5      nUR                  US	5        UR                  SX\R                  -  5      nUR                  US	5        UR                  US	5        UR                  XU5        UR                  UX5        X*R                  -  X:R                  -  -   XJR                  -  -   nU* U -  U
R                  -  nUU4UU4/nU Vs/ s H  nUR                  U
5      U-  PM     nnUUSS  -  n[        U	U5       VVs/ s H  u  nnUU-
  PM     nnn[!        XU	U5      nUR#                  / U5      u  nnU	S S n[%        ['        U	5      ['        U5      -
  5      nU
R                  U
R                  4 Vs/ s H  nUR)                  U
5      U-  PM     nn[!        XUUUUS9n U R#                  / U5      u  n!n[+        U* U -  U-  [-        US   5      -  U* U[/        US	   5      -  U[-        US	   5      -  -   -  [1        US   5      -  XR[/        US	   5      -  U[-        US	   5      -  -   -  UU/5      n"[+        U* U -  U-  [-        US   5      -  S	S	/5      n#[3        UR5                  5       5      [3        U"R5                  5       5      :X  d   e[3        U!R5                  5       5      [3        U#R5                  5       5      :X  d   eg s  snf s  snnf s  snf )Nzg m Px Py Pz R tzq:5r   r   zu:5r   B_primer   r   r   r   r   r    r   r!   r"   r   r   )r,   r-   )r	   r   r   r3   r4   r   r5   r   r:   r;   r6   r>   r/   r   r9   r1   r   rJ   r   setr=   r   r   r   r   r   rA   )$rQ   rP   PxPyPzr   rN   rW   rY   rZ   r   r   r   r   pOpRr   pCsR_C_hatR_Csr   rb   u_exprr[   r   r   km1fr1_u_indeprw   r   km2r   fr1_expectedfr2_expecteds$                                       r   test_sub_qdot2r    s    %%78A""uA	Q	'BuA 	sAkk)VadACC[9G#v1qtWYY'?@A	C!A$-A 
sBJJq! 
c1Q48ad133h.	/BJJq++b/&&q,-JJq! \\$"F
NN1a 

433
'CKK1KK1 OOB1
sA! fr##vo33&G2a48Dw#t-F
 .//Qrall1o"QF/
bfF"1f~
.~eb!26~C
.
aAs
#C  V,FC eGQ#g,&'E()QSSz	2z&**Q-"
zB	2
aGS"'bBC  V,FC	
1Qs1Q4y	
Bs1Q4yL2c!A$i<'(QqT2	c!A$i<"S1Y,&'

 L 	
1Qs1Q4y		 L SZZ\"h|/B/B/D&EEFESZZ\"h|/B/B/D&EEFE5 0
. 
3s   PPPN)sympyr   r   r   r   r   r   r	   sympy.simplify.simplifyr
   sympy.simplify.trigsimpr   sympy.solvers.solversr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   r   r   r       r   <module>r
     sF    ; ; ; , , '@ @ @y*xa:FVLpNGr	  