+
    i)T                         ^ RI Ht ^ RI HtHtHtHtHtHtHtH	t	H
t
Ht ^ RIHt ^ RIHtHtHtHtHtHtHtHtHt ^ RIHt R tR tR tR	 tR
 tR tR t R t!R t"R t#R# )    )solve)
cosexpandMatrixsinsymbolstansqrtSzeroseye)simplify)	dynamicsymbolsReferenceFramePoint	RigidBodyKanesMethodinertiaParticledotfind_dynamicsymbols)raisesc            	      P  aaaaaa	 \        R 4      w  rp\        R4      w  oo	SP                  \        P                  4      S	,
          .o\	        R4      \        R4      uopVP                  S^ 4       \        R\        R4      V4      oSP                  P                  W0\        S4      SP                  ,          \        S4      SP                  ,          ,
          ,          4       SP                  V) V,          SP                  ,          3o\        \        VVVVVV	3R l4       R# )zl m gq uNOPc            	      .   < \        SS.S.SS.S .R 7      # )bodies	forcelistr   )Fr   r   kdqus   e/var/www/html/photoedit/myenv/lib/python3.14/site-packages/sympy/physics/mechanics/tests/test_kane.py<lambda>*test_invalid_coordinates.<locals>.<lambda>   s    {1qcA3A367S :    N)r   diffr   _tr   r   set_velr   pointset_posr   xr   yr   
ValueError)
lmgr   r#   r   r   r$   r%   r&   s
       @@@@@@r'   test_invalid_coordinatesr6      s    gGA!5>DAq
&&""
#a
'	(B#c
DAqIIaOeCj!$AGGOOACFQSSL3q6ACC<789	
1"q&133,A
: : : ;r*   c                     \        R 4      w  r\        R ^4      w  r#\        R4      w  rEp\        R4      p\        R4      pVP	                  WqVP
                  ,          4       W!,
          .p	W) V ,          WQ,          ,
          VP
                  ,          3.p
\        RW4      pV.p\        Wp.V.V	4      pVP                  W4       VP                  V8X  g   Q hVP                  V
8X  g   Q hVP                  pVP                  pVP                  4       V,          p\        V^ ,          4      \        W,          W,          ,           ) V,          4      8X  g   Q h\        VP!                  4       VP"                  P%                  VP&                  4      ,
          4      \)        ^^4      8X  g   Q hVP+                  RR7      ^ ,          \-        ^ ^.V) V,          V) V,          ..4      8X  g   Q hR# )r   m c kr   r   paTA_and_BN)r   r   r   r   r-   r0   r   r   kanes_equationsr    loadsmass_matrixforcinginvr   r   rhsmass_matrix_fullLUsolveforcing_fullr   	linearizer   )r%   r&   qdudr4   ckr   r   r$   FLr9   BLKMMMr?   rA   s                    r'   test_one_dofrN      s    % DAE1%FBgGA!sAc
AIIaQSS
&BrAv~$
%	&B	$	B
B	QaS"	%Br99??88r>>	BjjG
&&(W
C#a&>Vaeaem$4q$89999BFFH''//@A BEJ1a[Q Q Q LLL(+v1v1qbd|6L/MMNMr*   c            	      
  aaaaaa \        R 4      w  oooo\        R ^4      w  rr#\        R4      w  rErgp\        R4      o\        R4      p	\        R4      p
V	P	                  SSSP
                  ,          4       V
P	                  SSS,           SP
                  ,          4       V ^,          S^,          ,
          ^V,          ^S,          ,
          .oW) S,          VS,          ,
          VS,          ,           VS,          ,           SP
                  ,          3W) S,          VS,          ,
          SP
                  ,          3.p\        RW4      p\        RW4      pW.p\        SSS.SS.SR7      pVP                  W4       VP                  pVP                  pVP                  4       V,          p\        V^ ,          4      \        V) S,          VS,          ,
          VS,          ,           VS,          ,           V,          4      8X  g   Q h\        V^,          4      \        VS,          VS,          ,           ^V,          S,          ,
          ^V,          S,          ,
          V,          4      8X  g   Q hVP                  '       g   Q hVP                  \        ^4      8X  g   Q hRVn        VP                  \!        \#        ^4      ^,          ^ .^ ^..4      8X  g   Q hR F^  pVVn        \%        VP'                  4       VP(                  P+                  VP,                  4      ,
          4      \/        ^^4      8X  d   K^  Q h	  V S^,          ,
          \1        V4      \3        S4      ,
          .o\5        \6        VVVVVV3R	 l4       R
# )q1 q2 u1 u2m c1 c2 k1 k2r   P1P2pa1pa2q_indu_indkd_eqsFc                  *   < \        S SS.SS.SR 7      # )rV   r"   )r   r$   q1q2u1u2s   r'   r(   test_two_dof.<locals>.<lambda>p   s    {1RH24b" Fr*   N)FT)r   r   r   r   r-   r0   r   r   r<   r>   r?   r@   r   explicit_kinematicsmass_matrix_kinr   r   r   r   rA   rB   rC   rD   r   r   r   r   r2   )q1dq2du1du2dr4   c1c2k1k2rR   rS   rJ   rT   rU   rK   rL   rM   r?   rA   r`   r   r$   r[   r\   r]   r^   s                       @@@@@@r'   test_two_dofrj   9   s   
 $M2NBB'q9Cc0A22sA	tB	tBJJq"qss(JJq27acc/" a%"Q$,#"	%B b27"R"W,rBw6!##=
>c
G"WGF A 
B
5"
 C
5"
 C
B
 
Qr2hr2hr	BBr	BjjG
&&(W
C#a&>VbS2XR%7"r'%ABG%KQ$NOOOO#a&>VR"WrBw%6R"%Dq$&H')+H, &,/0%1 2 2 2 2 !!!!!Q''' #B!A$q&!q!f(=!>>>>  -!4++33BOODE FINqRSU 	U U  - A+s3x#b')	*B
: F F Gr*   c                     \        R 4      w  r\        R ^4      w  r#\        R4      w  rEp\        R4      p\        R4      pVP	                  Wu) V,          \        V 4      ,          VP                  ,          WQ,          \        V 4      ,          VP                  ,          ,           4       W!,
          .p	WV,          VP                  ,          3.p
\        RW4      pV.p\        Wp.V.V	4      pVP                  W4       VP                  pVP                  pVP                  4       V,          pVP                  4        \!        V^ ,          4      \!        V) V,          \        V 4      ,          4      8X  g   Q h\        VP#                  4       VP$                  P'                  VP(                  4      ,
          4      \+        ^^4      8X  g   Q hR# )r   zm l gr   r   r9   N)r   r   r   r   r-   r   r0   r   r1   r   r   r<   r>   r?   r@   r   r   rA   rB   rC   rD   r   )r%   r&   rF   rG   r4   r3   r5   r   r   r$   rJ   r9   rK   rL   rM   r?   rA   s                    r'   	test_pendrl   s   si   % DAE1%FBgGA!sAc
AIIaa#a&133&Q!##)==>
&B!eacck
	B	$	B
B	QaS"	%Br	BjjG
&&(W
CLLN#a&>VQBFSVO4444BFFH''//@A BEJ1a[Q Q Qr*   c                     \        R 4      w  rr#rE\        R ^4      w  rgrr\        R4      w  rp\        R4      pVP                  RRWP                  .4      pVP                  RRVVP
                  .4      pVP                  RRVVP                  .4      pVP                  V4      pVP                  WVP
                  ,          VVP                  ,          ,           VVP                  ,          ,           4       \        R4      pVP                  V^ 4       VP                  RVVP                  ,          4      pVP                  VVV4       \        VV^,          V^,          ,          V^,          V^,          ,          V^,          V^,          ,          4      pV Uu. uF%  p\        VP                  V4      V,
          V4      NK'  	  ppVV) V,          VP                  ,          3.p\        R	VVVVV34      pV.p\!        WW.W4V.VR
7      pVP#                  VV4       VP$                  pVP&                  pVP)                  4       V,          pVP+                  4       p VP-                  V 4      pVP/                  4        VP1                  4       \3        ^V,          V,          V,          V^,          V,          \5        V4      ,          ,
          ^V,          \7        V4      ,          ,           ^V,          ,          RV,          V,          ^,          VRV,          V\5        V4      ,          ,           ,          .4      P1                  4       8X  g   Q h\/        VP9                  4       VP:                  P=                  VP>                  4      ,
          4      \A        ^^4      8X  g   Q hVPC                  RR7      ^ ,          p!V!P-                  V^V^V^/4      P-                  V ^ V^ V^ V^ V^ /4      p"^ RI"p#V#PG                  V"P-                  V^\I        ^4      ,          /4      4      PK                  4       \L        PN                  ^/8X  g   Q hR# u upi )q1 q2 q3 u1 u2 u3r m gr   YAxisLRCDmcBodyDrV   Tr:   N)(r   r   r   	orientnewzr0   r1   
ang_vel_inset_ang_velr   r-   	locatenewv2pt_theoryr   r   r   r   r<   r>   r?   r@   kindiffdictsubsr   r   r   r	   r   rA   rB   rC   rD   r   rE   sympysympifyr
   	eigenvalsr   Zero)$r[   r\   q3r]   r^   u3rb   rc   q3drd   re   u3drr4   r5   r   rp   rr   rs   w_R_N_qdrt   ru   Iuvr$   	ForceListrv   BodyListrL   rM   r?   rA   kddA	A_uprightr   s$                                       r'   test_rolling_discr      s    ,,?@BBB#12Eq#I CcgGA! 	sA	C"cc+A	C"acc+A	C"acc+A||AHMM!!##XQSS(2834 	c
AIIaO
++eQW
%COOAq! 	1q51a4<QAq1uq!t|<A 9:	:"#all1o("
-B	: sQw}%&IgsAq1c(3EwH 
Q2l22,r	JBx+	BjjG
&&(W
C
..
C
((3-CLLN::<6AbDGAIAaB$?	!CG%c$rE"HQJBrEBs2wJ,>(?#A BBH&(K K KBFFH''//@A BEJ1a[Q Q Q 	T"1%A1aAq)*//QAr1b!RQR0STI==Qa[(9:;EEGAFFTU;VVVG 
;s   +Q
c            	      "   \        R 4      w  rr#rE\        R ^4      w  rgrr\        R4      w  rr\        R^4      w  pp\        R4      w  ppp\        R4      pVP                  RRV VP                  .4      pVP                  RRVVP
                  .4      pVP                  RRVVP                  .4      pVP                  V4      pVP                  VVVP
                  ,          VVP                  ,          ,           VVP                  ,          ,           4       \        R	4      pVP                  VVVP
                  ,          VVP                  VP
                  ,          ,          ,           4       VP                  R
VVP                  ,          4      pVP                  VVV4       VP                  VVV4       \        VV^,          V^,          ,          V^,          V^,          ,          V^,          V^,          ,          4      pV Uu. uF%  p\        VP                  V4      V,
          V4      NK'  	  ppVV) V,          VP                  ,          3VVVP
                  ,          VVP                  VP
                  ,          ,          ,           3.p\!        RVVVVV34      p V .p!\#        VWV.W4W\V.VR7      p"V"P%                  V!V4      w  p#p$V#P'                  V^ V^ /4      P'                  V^ V^ /4      p#V$P'                  V^ V^ /4      P'                  V^ V^ /4      p$\#        VWV.W4V.VW.R7      p%V%P%                  V!V4      w  p&p'V&P'                  V^ V^ /4      P'                  V^ V^ /4      p&V'P'                  V^ V^ /4      P'                  V^ V^ /4      p'V$P)                  4        V'P)                  4        V#V&,
          P+                  4       \-        . RO4      8X  g   Q hV$V',
          P+                  4       \-        . RO4      8X  g   Q hR# u upi )rn   zu4, u5, f1, f2zu4, u5ro   r   rp   rq   rr   rs   rt   ru   rv   rV   )rW   rX   rY   u_auxiliaryN)r   r   r   r   r   )r   r   r   rx   ry   r0   r1   rz   r{   r   r-   r|   r}   a2pt_theoryr   r   r   r   r<   r   r   r   r   )(r[   r\   r   r]   r^   r   rb   rc   r   rd   re   r   u4u5f1f2u4du5dr   r4   r5   r   rp   rr   rs   r   rt   ru   r   r   r$   r   rv   r   rL   frfrstarKM2fr2frstar2s(                                           r'   test_auxr      s    ,,?@BBB#12Eq#I Cc#$45NBBh*HCgGAq!sA	C"acc+A	C"acc+A	C"acc+A||AHMM!R!##XQSS(2834c
AIIaaccB!##),,-
++eQW
%COOAq!OOAq!1q51a4<QAq1uq!t|<A89	:"#all1o("
-B	:sQw}%28bACC!##I6F+F'GHIgsAq1c(3EwH	Qrrl2222F
 B%%h	:LR	#q#q!	"	'	'QA	7B[[#q#q)*//QA?F
a|BB<#%(,C((9=NS'
((CC#
$
)
)2q"a.
9CllCC+,112q"a.AG
OOH&"9999W$$&&*AAAA- 
;s   	+Pc                     \        R 4      p \        R4      w  r\        R4      w  r4p\        R4      p\        R4      w  rxp	\        R4      w  r\        R^4      w  r\        R4      w  r\        R^4      w  pp\        R4      p\        R4      pVP                  VR	V) VP                  .4       VP                  VV) VP                  ,          4       \        R
4      pVP                  RV
VP                  ,          4      pVP                  RVVP                  ,          4      pVP                  V^ 4       VP                  VVVP                  ,          4       VP                  VVV4       \        RVV4      p\        RVVV\        VWxV	4      V34      pW,
          W,
          .pVV.pVVP                  ) V ,          V,          3VVP                  ) V ,          V,          3VVP                  ) V,          W,
          ,          3VVP                  V,          3.p\        VW.W.V4      pVP!                  VV4      w  ppVP"                  pVR,          V	8X  g   Q hR# )r5   zk lsza mA mCr#   zIx Iy Izzq1 q2zu1 u2r   r   rq   r   rt   AoCartPendulumN)   r   )r   r   r   orientry   r{   r   r|   r0   r1   r-   r}   r   r   r   r   r<   rB   ) gravityrI   lsamAmCr#   IxIyIzr[   r\   rb   rc   r]   r^   rd   re   r   r   r   rt   r   r   r   kindiffsbodyList	forceListkmr   r   mms                                    r'   test_parallel_axisr   	  s    clGFOEA	"IA2sA$JBB G$FBgq)HCG$FBgq)HC 	sAsAHHQ"acc
#MM!bS133Y 	c
A 	
Cacc"A	
T1qss7	#B IIaOIIaaccNN1aFAr"DRBBB1G0KLH #(#HhHqssdWnr)*accTG^b()accTAX)*QSS1WI
 
Q2(H	5B%%h	:LR			Bd8r>>r*   c            	      |  a  \        R 4      w  r\        R ^4      w  r#\        R4      w  rEp\        R4      p\        R4      pVP	                  WqVP
                  ,          4       W!,
          .p	W) V ,          WQ,          ,
          VP
                  ,          3.p
\        RW4      pV.p\        Wp.V.V	4      o S P                  V4      ^ ,          \        ^ .4      8X  g   Q hS P                  VRR7      ^ ,          \        ^ .4      8X  g   Q hS P                  VRR7      ^ ,          \        ^ .4      8X  g   Q hS P                  V4      ^ ,          \        ^ .4      8X  g   Q hS P                  V. 4      ^ ,          \        ^ .4      8X  g   Q h\        \        V 3R l4       \        Wp.V.WV
R	7      o S P                  4       ^ ,          \        V) V,          W`,          ,
          .4      8X  g   Q h\        R
4      w  rpp\        R
^4      w  pppp\        R4      w  ppppp\        R4      p\        R4      p\        R4      pVP	                  WVP
                  ,          4       VP	                  WV,           VP
                  ,          4       VV,
          VV,
          .p	VV) V,          VV,          ,
          VV,          ,           VV,          ,           VP
                  ,          3VV) V,          VV,          ,
          VP
                  ,          33p
\        RVV4      p\        RVV4      pVV3p\        W}V.VV.V	R7      o S P                  W4       S P                  pS P                  pVP                  4       V,          p\        V^ ,          4      \        V) V,          VV,          ,
          VV,          ,           VV,          ,           V,          4      8X  g   Q h\        V^,          4      \        VV,          VV,          ,           ^V,          V,          ,
          ^V,          V,          ,
          V,          4      8X  g   Q hR# )r   r8   r   r   r9   N)r    r=   )r=   c                  &   < S P                  R 4      # )z	bad input)_form_fr)rL   s   r'   r(   #test_input_format.<locals>.<lambda>[  s    r{{;7r*   r   rP   rQ   rR   rS   rT   rU   rV   )r   r   r   r   r-   r0   r   r   r<   r   r   r2   r>   r?   r@   r   )!r%   r&   rF   rG   r4   rH   rI   r   r   r$   rJ   r9   rK   r[   r\   r]   r^   rb   rc   rd   re   rf   rg   rh   ri   rR   rS   rT   rU   rM   r?   rA   rL   s!                                   @r'   test_input_formatr   A  s   % DAE1%FBgGA!sAc
AIIaQSS
&BrAv~$
%	&B	$	B
B	QaS"	%Bb!!$s333Rt4Q761#;FFFb-a0FA3K???b!!$s333b"%a(FA3K777
:78 
QaS"2	>B"fqbdQSj\&:::: $M2NBB'q9Cc30Ar2r2sA	tB	tBJJqqss(JJq7acc/"
(C"H	Bb27"R"W,rBw6!##=
>rc
G"WGF A 
B
5"a
 C
5"a
 C
sB	Q2hr2hr	BB r	BjjG
&&(W
C#a&>VbS2XR%7"r'%ABG%KQ$NOOOO#a&>VR"WrBw%6R"%Dq$&H')+H, &,/0%1 2 2 2 2r*   c                  \   \        R 4      p \        R4      pVP                  V ^ 4       \        RRR7      pV P	                  RRV4      p\        R4      pVP                  RV^ ,          VP                  ,          V^,          VP                  ,          ,           V^,          VP                  ,          ,           4      pVR	,          V,           pV^ ,          .p. pVP                  V 4      p	\        R
4      w  rpVP                  W
VP                  ,          WP                  ,          ,           WP                  ,          ,           4       VP                  V 4      V	,
          P                  4       pVWP                  ,          WP                  ,          WP                  ,          .,          pVP                  V 4      p\        R4      w  pppVP                  WVP                  ,          VVP                  ,          ,           VVP                  ,          ,           4       VP                  V 4      V,
          pVVVP                  ,          VVP                  ,          VVP                  ,          .,          pVVVWV.p\        V4      pVP                  V,          ^ ,          ^,
          .pVVP                  VP                  4       ,          ^ ,          .,           p \!        WVVVV. . . RR7
      p\%        R4      p\'        T.R Uu. uF/  p\)        RT 24      T^ ,          T^,          8X  d   ^MR,          NK1  	  upO5!  pTP+                  \)        R4      ^ \)        R4      ^ /4      p\-        RYSTTT34      pT.pTP.                  TP0                  \)        R4      ,          T P                  ,          3TP2                  \        R4      TP                  ,          3TP.                  \        R4      TP                  ,          3.pTP5                  TT4       \7        TP8                   Uu. uF  pTP;                  4       NK  	  upTP<                   Uu. uF  pTP;                  4       NK  	  up,           4      pTP>                  p TP@                  p!RTn!        \7        TP8                   Uu. uF  pTP;                  4       NK  	  upTP<                   Uu. uF  pTP;                  4       NK  	  up,           4      p"TP@                  p#TT",          R8  g   Q hT TPD                  P                  4       ,          T!,
          \        T4      8X  g   Q hT#p$\        ^ T
) T) T) .T
^ Y) .Y) ^ T
.YT
) ^ ..4      T,          ^,          p%T%^ ,          T$R&   T%^,          T$^ &   T%^,          T$^&   T%^,          T$^&   \G        T^ ,          T^ ,          4      ^,          p&\        T T$,          4      P+                  T^ ,          T&/4      p'T'T!8X  g   Q hR#   \"         d   pThRp?ii ; iu upi u upi u upi u upi u upi )NEDNED_oz
lambda_0:4T)realB
QuaternionzB_x:zB_cm:   NNzP Q RzU V WF)q_dependentrY   configuration_constraintsvelocity_constraintsu_dependentr   r`   NM_BJ_B_J_B_xyJ_B_yzRBr5   T_zF_zg?)xxyyzzxyyzxz)$r   r   r-   r   rx   r|   r0   r1   ry   rz   r{   r   velr   Tr+   r   	Exceptionr   r   r   r   r   
masscentermassframer<   sumrD   	count_opsrB   ra   forcing_kinr`   r%   r   )(r   r   q_attr   q_posr   rW   q_depkinematic_eqs	B_ang_velr   Qrs   B_ang_vel_kdB_cm_velUVWB_ref_vel_kdrX   	q_att_vecconfig_consrL   er   axJ_Br   rigid_bodies
force_listr0   n_ops_implicitmass_matrix_kin_implicitforcing_kin_implicitn_ops_explicitforcing_kin_explicitqdot_candidatequat_dot_textbooklambda_0_sollhs_candidates(                                           r'   test_implicit_kinematicsr   ~  sr    
C'NE	MM#q <d3Ec</A 7#E??658ACC<%(133,#>q!###MND"IE1XJEM S!IW%GA!MM#uqu}qu,-LL%	1;;=L ssssss M xx}HW%GAq!LLeaemae+, HHSMH,L "QSS("QSS("QSS( M 1aAE uIKK	)1-12K!ikkINN4D&Da%H$IIMU &1 " %
 %.C
! =:<: 4t~BqERUNqCC:< =C
((AhKAhK3
4C	4#T{	3B4L 
#suu,-	>%(,-	u-acc12J |Z0  "010 " 3 34 31 34	5N
  "11>>!B "010 " 3 34 31 34	5N >>N*S000 %rttyy{25IIfUbNcccc
 *N	
QBQB	
QB	
BQ	
Q	  
 

  +1-N2*1-N1*1-N1*1-N1 Q16q9L5FGLLiXYl\hMijM0000Q  
<& 	14 	14s6   X  5X
$XX
%X$X)
 XXXc                     \        R 4      w  rr#\        R4      w  rErgr\        R4      p
\        R4      pVP                  WWV3R4       VP	                  V
4      pVP                  VP                  4      V,
          VP                  VP                  4      V,
          VP                  VP                  4      V	,
          .p\        R4      pVP                  V
^ 4       VP                  RV) VP                  ,          4      pVP                  V^ 4       \        RW4      p\        WWV.WxV	.VV.W) V ,          V
P                  ,          3.R7      pVP                  4        W!^,          ,          \        . R
O. RO. RO.4      ,          p\        W,          V \!        V4      \!        V4      ,          \!        V4      \#        V4      ,          \#        V4      ,          ,
          ,          W,          V	,          ,
          ,          .^ .W,          V ) \!        V4      \#        V4      ,          \!        V4      \!        V4      ,          \#        V4      ,          ,           ,          W,          V,          ,           ,          ..4      p\%        VP&                  4      P)                  WEWgW04      '       g   Q h\+        VP,                  V,
          4      \/        ^^4      8X  g   Q h\+        VP&                  V,
          4      \/        ^^4      8X  g   Q hR	# )zg l m cz	q1:4 u1:4r   r   zxyr   Por   r   N)r   r   r   )r   r   r   )r   r   r   )r   r   r   orient_body_fixedrz   r   r0   r1   ry   r   r-   r|   r   r   r<   r   r   r   r   r?   issubsetr   r>   r   )r5   r3   r4   rH   r[   r\   r   r]   r^   r   r   r   N_w_Akdesr   r   r   kaneexpected_mdexpected_fds                       r'   test_issue_24887r    sF   #JA!+K8BBBsAsA</LLOEIIaccNR133"!4eiinr6IJDc
AIIaO	
TA28	$BJJq!bAqr,dA3#%rAv|"4!57D1f*vy)Y&GHHK	
aRR3r73r7?3r7#::;ad2gE	FG	
acA2s2ws2wRRR)@@AADGKLMO PK t||,55rrr6NOOOOD$${23uQ{BBBDLL;./5A;>>>r*   N)$r   r   r   r   r   r   r   r	   r
   r   r   r   sympy.simplify.simplifyr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   sympy.testing.pytestr   r6   rN   rj   rl   r   r   r   r   r   r   r*   r'   <module>r	     sn    , , , ,? ? ? (;O@8GtQ0GWT1Bh6p:2zQ1f?r*   