
    \h)T                         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 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  S SKJr  S rS rS rS	 rS
 rS rS r S r!S r"S r#g)    )solve)
cosexpandMatrixsinsymbolstansqrtSzeroseye)simplify)	dynamicsymbolsReferenceFramePoint	RigidBodyKanesMethodinertiaParticledotfind_dynamicsymbols)raisesc            	        ^^^^^^	 [        S5      u  pn[        S5      u  mm	TR                  [        R                  5      T	-
  /m[	        S5      [        S5      smnUR                  TS5        [        S[        S5      U5      mTR                  R                  X0[        T5      TR                  -  [        T5      TR                  -  -
  -  5        TR                  U* U-  TR                  -  4m[        [        UUUUUU	4S j5        g )Nzl m gq uNOr   Pc            	      (   > [        TT/T/TT/T /S9$ )Nbodies	forcelistr   )Fr   r   kdqus   _/var/www/auris/envauris/lib/python3.13/site-packages/sympy/physics/mechanics/tests/test_kane.py<lambda>*test_invalid_coordinates.<locals>.<lambda>   s    {1qcA3A367S :    )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                  D   [        S5      u  p[        SS5      u  p#[        S5      u  pEn[        S5      n[        S5      nUR	                  XqUR
                  -  5        X!-
  /n	X* U -  XQ-  -
  UR
                  -  4/n
[        SX5      nU/n[        Xp/U/U	5      nUR                  X5        UR                  U:X  d   eUR                  U
:X  d   eUR                  nUR                  nUR                  5       U-  n[        US   5      [        X-  X-  -   * U-  5      :X  d   e[        UR!                  5       UR"                  R%                  UR&                  5      -
  5      [)        SS5      :X  d   eUR+                  S	S
9S   [-        SS/U* U-  U* U-  //5      :X  d   eg )Nr      m c kr   r   par      TA_and_B)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$   FLr:   BLKMMMrA   rC   s                    r'   test_one_dofrP      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            	      ~  ^^^^^^ [        S5      u  mmmm[        SS5      u  pp#[        S5      u  pEpgn[        S5      m[        S5      n	[        S5      n
U	R	                  TTTR
                  -  5        U
R	                  TTT-   TR
                  -  5        U S-  TS-  -
  SU-  ST-  -
  /mX* T-  UT-  -
  UT-  -   UT-  -   TR
                  -  4X* T-  UT-  -
  TR
                  -  4/n[        SX5      n[        S	X5      nX/n[        TTT/TT/TS
9nUR                  X5        UR                  nUR                  nUR                  5       U-  n[        US   5      [        U* T-  UT-  -
  UT-  -   UT-  -   U-  5      :X  d   e[        US   5      [        UT-  UT-  -   SU-  T-  -
  SU-  T-  -
  U-  5      :X  d   eUR                  (       d   eUR                  [        S5      :X  d   eSUl        UR                  [!        [#        S5      S-  S/SS//5      :X  d   eS HY  nUUl        [%        UR'                  5       UR(                  R+                  UR,                  5      -
  5      [/        SS5      :X  a  MY   e   U TS-  -
  [1        U5      [3        T5      -
  /m[5        [6        UUUUUU4S j5        g )Nq1 q2 u1 u2r8   m c1 c2 k1 k2r   P1P2r;   pa1pa2q_indu_indkd_eqsr   F)FT   c                  $   > [        T TT/TT/TS9$ )NrX   r"   )r   r$   q1q2u1u2s   r'   r(   test_two_dof.<locals>.<lambda>p   s    {1RH24b" Fr*   )r   r   r   r   r-   r0   r   r   r>   r@   rA   rB   r   explicit_kinematicsmass_matrix_kinr   r   r   r   rC   rD   rE   rF   r   r   r   r   r2   )q1dq2du1du2dr4   c1c2k1k2rT   rU   rL   rV   rW   rM   rN   rO   rA   rC   rc   r   r$   r^   r_   r`   ra   s                       @@@@@@r'   test_two_dofrm   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                     [        S5      u  p[        SS5      u  p#[        S5      u  pEn[        S5      n[        S5      nUR	                  Xu* U-  [        U 5      -  UR                  -  XQ-  [        U 5      -  UR                  -  -   5        X!-
  /n	XU-  UR                  -  4/n
[        SX5      nU/n[        Xp/U/U	5      nUR                  X5        UR                  nUR                  nUR                  5       U-  nUR                  5         [!        US   5      [!        U* U-  [        U 5      -  5      :X  d   e[        UR#                  5       UR$                  R'                  UR(                  5      -
  5      [+        SS5      :X  d   eg )	Nr   r8   zm l gr   r   r:   r   r;   )r   r   r   r   r-   r   r0   r   r1   r   r   r>   r@   rA   rB   r   r   rC   rD   rE   rF   r   )r%   r&   rH   rI   r4   r3   r5   r   r   r$   rL   r:   rM   rN   rO   rA   rC   s                    r'   	test_pendro   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                  D   [        S5      u  pp#pE[        SS5      u  pgpp[        S5      u  pn[        S5      nUR                  SSXR                  /5      nUR                  SSUUR
                  /5      nUR                  SSUUR                  /5      nUR                  U5      nUR                  XUR
                  -  UUR                  -  -   UUR                  -  -   5        [        S	5      nUR                  US
5        UR                  SUUR                  -  5      nUR                  UUU5        [        UUS-  US-  -  US-  US-  -  US-  US-  -  5      nU Vs/ s H!  n[        UR                  U5      U-
  U5      PM#     nnUU* U-  UR                  -  4/n[        SUUUUU45      nU/n[!        XX/X4U/US9nUR#                  UU5        UR$                  nUR&                  nUR)                  5       U-  nUR+                  5       n UR-                  U 5      nUR/                  5         UR1                  5       [3        SU-  U-  U-  US-  U-  [5        U5      -  -
  SU-  [7        U5      -  -   SU-  -  SU-  U-  S-  USU-  U[5        U5      -  -   -  /5      R1                  5       :X  d   e[/        UR9                  5       UR:                  R=                  UR>                  5      -
  5      [A        SS5      :X  d   eURC                  SS9S
   n!U!R-                  USUSUS05      R-                  U S
US
US
US
US
05      n"S
S K"n#U#RG                  U"R-                  US[I        S5      -  05      5      RK                  5       [L        RN                  S0:X  d   eg s  snf )Nq1 q2 q3 u1 u2 u3r8   r m gr   YAxisLRCr   Dmcr\   r;   BodyDrX            Tr<   )(r   r   r   	orientnewzr0   r1   
ang_vel_inset_ang_velr   r-   	locatenewv2pt_theoryr   r   r   r   r>   r@   rA   rB   kindiffdictsubsr   r   r   r	   r   rC   rD   rE   rF   r   rG   sympysympifyr
   	eigenvalsr   Zero)$r^   r_   q3r`   ra   u3re   rf   q3drg   rh   u3drr4   r5   r   rs   ru   rv   w_R_N_qdrw   rx   Iuvr$   	ForceListry   BodyListrN   rO   rA   rC   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    (Nc            	      2   [        S5      u  pp#pE[        SS5      u  pgpp[        S5      u  pp[        SS5      u  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                  S
SUUR                  /5      nUR                  U5      nUR                  UUUR
                  -  UUR                  -  -   UUR                  -  -   5        [        S5      nUR                  UUUR
                  -  UUR                  UR
                  -  -  -   5        UR                  SUUR                  -  5      nUR                  UUU5        UR                  UUU5        [        UUS-  US-  -  US-  US-  -  US-  US-  -  5      nU Vs/ s H!  n[        UR                  U5      U-
  U5      PM#     nnUU* U-  UR                  -  4UUUR
                  -  UUR                  UR
                  -  -  -   4/n[!        SUUUUU45      n U /n![#        UXU/X4X\U/US9n"U"R%                  U!U5      u  n#n$U#R'                  USUS05      R'                  USUS05      n#U$R'                  USUS05      R'                  USUS05      n$[#        UXU/X4U/UX/S9n%U%R%                  U!U5      u  n&n'U&R'                  USUS05      R'                  USUS05      n&U'R'                  USUS05      R'                  USUS05      n'U$R)                  5         U'R)                  5         U#U&-
  R+                  5       [-        / SQ5      :X  d   eU$U'-
  R+                  5       [-        / SQ5      :X  d   eg s  snf )Nrq   r8   zu4, u5, f1, f2zu4, u5rr   r   rs   rt   ru   rv   rw   rx   r\   r;   ry   rX   r   )rY   rZ   r[   u_auxiliary)r   r   r   r   r   )r   r   r   r~   r   r0   r1   r   r   r   r-   r   r   a2pt_theoryr   r   r   r   r>   r   r   r   r   )(r^   r_   r   r`   ra   r   re   rf   r   rg   rh   r   u4u5f1f2u4du5dr   r4   r5   r   rs   ru   rv   r   rw   rx   r   r   r$   r   ry   r   rN   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   =(Nc                  8   [        S5      n [        S5      u  p[        S5      u  p4n[        S5      n[        S5      u  pxn	[        S5      u  p[        SS5      u  p[        S5      u  p[        SS5      u  nn[        S	5      n[        S
5      nUR                  USU* UR                  /5        UR                  UU* UR                  -  5        [        S5      nUR                  SU
UR                  -  5      nUR                  SUUR                  -  5      nUR                  US5        UR                  UUUR                  -  5        UR                  UUU5        [        SUU5      n[        SUUU[        UXxU	5      U45      nX-
  X-
  /nUU/nUUR                  * U -  U-  4UUR                  * U -  U-  4UUR                  * U-  X-
  -  4UUR                  U-  4/n[        UX/X/U5      nUR!                  UU5      u  nnUR"                  nUS   U	:X  d   eg )Nr5   zk lsza mA mCr#   zIx Iy Izzq1 q2r8   zu1 u2r   r   rt   r   rw   Aor   CartPendulum)r}   r}   )r   r   r   orientr   r   r   r   r0   r1   r-   r   r   r   r   r   r>   rD   ) gravityrK   lsamAmCr#   IxIyIzr^   r_   re   rf   r`   ra   rg   rh   r   r   r   rw   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            	        ^  [        S5      u  p[        SS5      u  p#[        S5      u  pEn[        S5      n[        S5      nUR	                  XqUR
                  -  5        X!-
  /n	X* U -  XQ-  -
  UR
                  -  4/n
[        SX5      nU/n[        Xp/U/U	5      m T R                  U5      S   [        S/5      :X  d   eT R                  US S9S   [        S/5      :X  d   eT R                  US S	9S   [        S/5      :X  d   eT R                  U5      S   [        S/5      :X  d   eT R                  U/ 5      S   [        S/5      :X  d   e[        [        U 4S
 j5        [        Xp/U/XU
S9m T R                  5       S   [        U* U-  X`-  -
  /5      :X  d   e[        S5      u  pnn[        SS5      u  nnnn[        S5      u  nnnnn[        S5      n[        S5      n[        S5      nUR	                  XUR
                  -  5        UR	                  XU-   UR
                  -  5        UU-
  UU-
  /n	UU* U-  UU-  -
  UU-  -   UU-  -   UR
                  -  4UU* U-  UU-  -
  UR
                  -  44n
[        SUU5      n[        SUU5      nUU4n[        X}U/UU/U	S9m T R                  X5        T R                  nT R                  nUR                  5       U-  n[        US   5      [        U* U-  UU-  -
  UU-  -   UU-  -   U-  5      :X  d   e[        US   5      [        UU-  UU-  -   SU-  U-  -
  SU-  U-  -
  U-  5      :X  d   eg )Nr   r8   r9   r   r   r:   r   )r    r?   )r?   c                  &   > T R                  S5      $ )Nz	bad input)_form_fr)rN   s   r'   r(   #test_input_format.<locals>.<lambda>[  s    r{{;7r*   r   rR   rS   rT   rU   rV   rW   rX   r;   )r   r   r   r   r-   r0   r   r   r>   r   r   r2   r@   rA   rB   r   )!r%   r&   rH   rI   r4   rJ   rK   r   r   r$   rL   r:   rM   r^   r_   r`   ra   re   rf   rg   rh   ri   rj   rk   rl   rT   rU   rV   rW   rO   rA   rC   rN   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                  l
   [        S5      n [        S5      nUR                  U S5        [        SSS9nU R	                  SSU5      n[        S	5      nUR                  S
US   UR                  -  US   UR                  -  -   US   UR                  -  -   5      nUSS  U-   nUS   /n/ nUR                  U 5      n	[        S5      u  pnUR                  X
UR                  -  XR                  -  -   XR                  -  -   5        UR                  U 5      U	-
  R                  5       nUXR                  -  XR                  -  XR                  -  /-  nUR                  U 5      n[        S5      u  nnnUR                  XUR                  -  UUR                  -  -   UUR                  -  -   5        UR                  U 5      U-
  nUUUR                  -  UUR                  -  UUR                  -  /-  nUUUXU/n[        U5      nUR                  U-  S   S-
  /nUUR                  UR                  5       -  S   /-   n [!        XUUUU/ / / SS9
n[%        S5      n['        U/S Vs/ s H"  n[)        SU 35      US   US   :X  a  SOS-  PM$     snQ76 nUR+                  [)        S5      S[)        S5      S05      n[-        SXSUUU45      nU/nUR.                  UR0                  [)        S5      -  U R                  -  4UR2                  [        S5      UR                  -  4UR.                  [        S5      UR                  -  4/nUR5                  UU5        [7        UR8                   Vs/ s H  nUR;                  5       PM     snUR<                   Vs/ s H  nUR;                  5       PM     sn-   5      nUR>                  n UR@                  n!SUl!        [7        UR8                   Vs/ s H  nUR;                  5       PM     snUR<                   Vs/ s H  nUR;                  5       PM     sn-   5      n"UR@                  n#UU"-  S:  d   eU URD                  R                  5       -  U!-
  [        U5      :X  d   eU#n$[        SU
* U* U* /U
SX* /X* SU
/XU
* S//5      U-  S-  n%U%S   U$S'   U%S   U$S'   U%S   U$S'   U%S   U$S'   [G        US   US   5      S   n&[        U U$-  5      R+                  US   U&05      n'U'U!:X  d   eg ! ["         a  nUeS nAff = fs  snf s  snf s  snf s  snf s  snf )NNEDNED_or   z
lambda_0:4T)realB
QuaternionzB_x:zB_cmr8   r;   zP Q RzU V WF)q_dependentr[   configuration_constraintsvelocity_constraintsu_dependentr   rc   M_B)xxyyzzxyyzxzJ_B_J_B_xyJ_B_yzRBr5   T_zF_zg?r}   )$r   r   r-   r   r~   r   r0   r1   r   r   r   r   velr   Tr+   r   	Exceptionr   r   r   r   r   
masscentermassframer>   sumrF   	count_opsrD   rd   forcing_kinrc   r%   r   )(r   r   q_attr   q_posr   rY   q_depkinematic_eqs	B_ang_velr   Qrv   B_ang_vel_kdB_cm_velUVWB_ref_vel_kdrZ   	q_att_vecconfig_consrN   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   ~  sq    
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
((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    T	 ')T
-T"T'
,T,T1
	
TTTc                     [        S5      u  pp#[        S5      u  pEpgp[        S5      n
[        S5      nUR                  XXV4S5        UR	                  U
5      nUR                  UR                  5      U-
  UR                  UR                  5      U-
  UR                  UR                  5      U	-
  /n[        S5      nUR                  U
S5        UR                  SU* UR                  -  5      nUR                  US5        [        S	X5      n[        XXV/XxU	/UU/X* U -  U
R                  -  4/S
9nUR                  5         X!S-  -  [        / SQ/ SQ/ SQ/5      -  n[        X-  U [!        U5      [!        U5      -  [!        U5      [#        U5      -  [#        U5      -  -
  -  X-  U	-  -
  -  /S/X-  U * [!        U5      [#        U5      -  [!        U5      [!        U5      -  [#        U5      -  -   -  X-  U-  -   -  //5      n[%        UR&                  5      R)                  XEXgX15      (       d   e[+        UR,                  U-
  5      [/        SS5      :X  d   e[+        UR&                  U-
  5      [/        SS5      :X  d   eg )Nzg l m cz	q1:4 u1:4r   r   zxyr   r   Por   r   r;   )r8   r   r   )r   r   r   )r   r   r8   r}   r8   )r   r   r   orient_body_fixedr   r   r0   r1   r   r   r-   r   r   r   r>   r   r   r   r   rA   issubsetr   r@   r   )r5   r3   r4   rJ   r^   r_   r   r`   ra   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   rP   rm   ro   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*   