
    \h'                         S SK Jr  S SK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JrJrJrJrJrJr  S SK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   \RB                  r"S r#S r$S r%S r&S r'S r(S r)S r*g)    )expand)symbols)cossin)Matrix)trigsimp)	PinJointJointsMethod	RigidBodyParticleBodyKanesMethodPrismaticJointLagrangesMethodinertia)dynamicsymbolsReferenceFrame)raiseswarns_deprecated_sympy)zeros)lambdify)solvec                     [        5          [        S5      n [        S5      nS S S 5        [        SW W5      n[        S5      u  p4[	        S5      u  pVU R                  X@R                  -  5        [        5          [        X5      nS S S 5        WR                  U R                  :X  d   eUR                  X/:X  d   eUR                  U R                  X@R                  R                  -  4/:X  d   eUR                  [        U/5      :X  d   eUR                  [        U/5      :X  d   eUR                  [        XeR!                  5       -
  /5      :X  d   eUR#                  5       nU[        U* UR!                  5       -  //5      :X  d   eUR$                  [        U/S//5      :X  d   eUR&                  [        SS/SU//5      :X  d   e[)        UR*                  [,        5      (       d   eg ! , (       d  f       GN= f! , (       d  f       GN= f)NPCP1zC_ixx gz
q_P1, u_P1r      )r   r   r	   r   r   apply_forceyr
   framebodiesloads
masscenterqr   ukdesdiff	form_eomsforcing_fullmass_matrix_full
isinstancemethodr   )	r   r   PinC_ixxgr$   r%   r,   solns	            g/var/www/auris/envauris/lib/python3.13/site-packages/sympy/physics/mechanics/tests/test_jointsmethod.pytest_jointsmethodr2      s   		!II 
" 4A
Cy!HE,'DAMM!CC%		!a% 
"<<177"""==QF"""<<Q\\1WWYY;7888888vqc{"""88vqc{""";;&!ffh,0000D6UF1668O,-....&1#s"4444""fq!fq%j-A&BBBBfmm[1111' 
"	! 
"	!s   G+8G=+
G:=
Hc                     [        S5      u  pn[        S5      n[        SUS9n[        S5      n[	        S5      u  pg[        SX4XgUU * UR                  -  UR                  S9n[        5          [        X85      n	S S S 5        W	R                  R                  UR                  X-  UR                  -  45        U	R                  5         U	R                  5       n
U
S	   U* [        U5      -  U -  :X  d   eg ! , (       d  f       N= f)
Nl m gr   bmassb_frameq ur   )coordinatesspeedschild_interframechild_point
joint_axisr   )r   r   r   r   r   r	   xzr   r
   r"   appendr#   r(   rhsr   )lmr/   r   r5   r8   r$   r%   r   r,   rB   s              r1   &test_rigid_body_particle_compatibilityrE   *   s    gGA!#A1AY'G% DAagR'))^	=A		!a# 
"
LLquqss{34

**,Cq6aRAYq[    
"	!s   ,C22
D c                    ^^^ [        5          [        S5      m[        S5      n [        S5      nS S S 5        [        S5      u  p#[        STW U5      m[	        SU WU5      m[        5          [        [        UUU4S j5        S S S 5        [        STXS9m[	        SXUS9m[        5          [        [        UUU4S	 j5        S S S 5        [        STXU5      m[	        SXX#5      m[        5          [        [        UUU4S
 j5        S S S 5        g ! , (       d  f       N= f! , (       d  f       N= f! , (       d  f       Nu= f! , (       d  f       g = f)Nr   r   Tr9   r   P2c                     > [        T TT5      $ Nr
   r   r   rH   s   r1   <lambda>?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>C       <2r#:    )r;   c                     > [        T TT5      $ rJ   rK   rL   s   r1   rM   rN   H   rO   rP   c                     > [        T TT5      $ rJ   rK   rL   s   r1   rM   rN   M   rO   rP   )r   r   r   r	   r   r   
ValueError)r   rG   r$   r%   r   r   rH   s       @@@r1   -test_jointmethod_duplicate_coordinates_speedsrT   :   s    		!III 
" % DA	$1a	 B	aA	&B		!z:; 
" 
$1	'B	a1	-B		!z:; 
" 
$1	#B	aA	)B		!z:; 
"	!# 
"	! 
"	!
 
"	!
 
"	!s/   "D+D*D0.E
D
D-0
D>
Ec                     [        S5      u  p[        S5      u  p#[        S5      u  pEn[        5          [        S5      n[        SUS9n[        SUS9n	S S S 5        [	        SWWX U* UR
                  -  UR                  S	9n
[	        S
UW	X1U* U	R
                  -  UR                  S	9nUR                  XF-  UR
                  -  5        U	R                  XF-  UR
                  -  5        [        5          [        XzU5      nS S S 5        WR                  5         [        UR                  5      [        / SQ/ SQSSSUS-  -  U-  [        U5      -  SUS-  -  U-  -   US-  U-  [        U5      -  US-  U-  -   /SSUS-  U-  [        U5      -  US-  U-  -   US-  U-  //5      :X  d   e[        UR                  5      [        [        U/U/U* U-  U-  [!        X-   5      [!        U 5      -   -  Xe-  U-  [!        U 5      -  -
  US-  U-  SU-  U-   -  U-  [!        U5      -  -   /U* U-  U-  [!        X-   5      -  US-  U-  US-  -  [!        U5      -  -
  //5      5      :X  d   eg ! , (       d  f       GN= f! , (       d  f       GN~= f)Nzq1 q2zu1 u2zm l gr   r   r6   RJ1r;   r:   r=   r>   J2)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   )q1q2u1u2rD   rC   r/   r   PartPPartRrW   rY   r,   s                r1   $test_complete_simple_double_pendulumrb   O   s   G$FBG$FBgGA!		!ISq!Sq! 
" 
$5 bjQSS
:B	$uR bjUWW
>B 
ac!##g	ac!##g		!aR( 
"
&))*fl6B78!Qq!tVAXc"g=MPQRSUVRVPVWXPX=XZ[]^Z^_`Z`adegahZhklnokopqkqZq6r78!QT!VCG^aQRdSTf=TVWYZVZ[\V\6]6_ /` ` ` ` F''(HVbTB41"Q$q&RUVXV]R^adegahRhJi+,3q5R=K9;<a461R4"9;Mb;PQTUWQX;XKY JZ,-2a46#bg,+>AaAcRTgAU+U*V=X 6Y -Z Z Z Z) 
"	! 
"	!s    I#1I5#
I25
Jc                     [        S5      u  pp#[        S5      u  pEpgn[        5          [        S5      n	[        SUS9n
[        SUS9nS S S 5        [	        SW	W
XS9n[	        S	U
WXS9nU	R                  Xp-  U	R                  -  U
S
9  U	R                  XR-  U	R                  -  U
S
9  U
R                  X-  U	R                  -  US
9  U
R                  Xc-  U	R                  -  US
9  [        5          [        XU5      nS S S 5        WR                  5         UR                  nUR                  nUR                  U5      n[        US   5      [        U* U -  XR-  -
  X-  -   Xc-  -   U-  5      :X  d   e[        US   5      [        Xp-  XR-  -   SU-  U-  -
  SU-  U-  -
  U-  5      :X  d   eg ! , (       d  f       GNh= f! , (       d  f       N= f)Nzq1 q2 u1 u2zm c1 c2 k1 k2WB1r6   B2rW   )r:   r;   rY   )reaction_bodyr   r   rZ   )r   r   r   r   r   r   r?   r
   r(   mass_matrixforcingLUsolver   )r\   r]   r^   r_   rD   c1c2k1k2rd   re   rf   rW   rY   r,   MMri   rB   s                     r1   test_two_dof_jointsrp   k   s   #M2NBB0A22		!I$Q$Q 
" 
a	?B	b""	@BMM"%)2M.MM"%)2M.NN259BN/NN259BN/		!aR( 
"
			BnnG
**W
C#a&>VbS2X%7"'%ABG%KQ$NOOOO#a&>VRWrw%6R"%Dq$&H')+H, &,/0%1 2 2 2 2# 
"	! 
"	!s    F).F;)
F8;
G	c            
      .   [        S5      u  pn[        5          [        S5      n[        SUS9nS S S 5        [        S5      n[	        SWWUR                  [        5      UU * UR                  -  UR                  S9nU* U-  U -  [        U5      -  Ul
        [        5          [        X65      nS S S 5        WR                  [        5        UR                  5       nUS   U* [        U5      -  U -  :X  d   eg ! , (       d  f       N= f! , (       d  f       N`= f)	Nr4   r   r5   r6   r$   r   rX   r   )r   r   r   r   r	   r'   tr?   r@   r   potential_energyr
   r(   r   rB   r   )	rC   rD   r/   r   r5   r$   r   r,   rB   s	            r1   test_simple_pedulumrt      s    gGA!		!I1 
" 	sAa166!9!R!##X!##	7Aq1s1v-A		!a# 
"
_%
**,Cq6aRAYq[    
"	! 
"	!s   C5 D5
D
Dc                     [        S5      u	  pp#pEpgn[        S5      u  pp[        S5      n[        S5      n[        5          [	        SX[        XUS5      S9n[	        SX[        XXg5      S9n[	        S	5      nS S S 5        [        S
WWXU* UR                  -  UR                  S9n[        SUWXX2-
  UR                  -  UR                  S9nUR                  X-  UR                  -  5        UR                  X-  UR                  -  5        [        5          [        UUU5      nS S S 5        WR                  5         UR                  nUR                  nUR                  U5      nSU-  U-  U-  [        U
5      -  [!        U
5      -  SU-  U-  U-  [        U
5      -  [!        U
5      -  -   X-  U -  [        U	5      -  -
  X-  U-  [        U	5      -  -
  XE[        U
5      S-  -  -   U[!        U
5      S-  -  -   US-  U -  -   US-  U-  -   -  nUS   U-
  R#                  5       S:X  d   eXV-
  US-  -  [        U
5      -  [!        U
5      -  U-  nUS   U-
  R#                  5       S:X  d   eg ! , (       d  f       GN= f! , (       d  f       GN_= f)Nz)mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, gztheta phi omega alphaABrodr   )r7   r    central_inertiaplater   rW   )r:   r;   r=   r>   rY   )r:   r;   parent_pointr>   rZ   r   )r   r   r   r   r   r   r	   r@   r   r   r
   r(   rh   ri   rj   r   r   simplify)mAmBlAlBIAxxIBxxIByyIBzzr/   thetaphiomegaalpharv   rw   rx   rz   r   rW   rY   r,   ro   ri   rB   xds                            r1   test_chaos_pendulumr      s   078c0d-BBDA-.EFEsAsA		!5r#*1D!#<>W2%,Qd%ACI 
" 
$3E!cCEEkacc
;B	$U "3550SUU
DB OOBDH	bd133h		!aR( 
"
			BnnG
**W
C
t)e
e
#c#h
.S
9AHu<Lu<TWZ[^W_<_H= v{SZ/023&2+E
2JKOSC!W P"SXq[0P135q52:P>@BA
PK
LB FRK!!#q(((
+	!CH	,s3x	7$	>BFRK!!#q(((5 
"	! 
"	!s    7I*I<*
I9<
Jc                  @  ^) [        S5      u  pp#pE[        S5      u  pgpn
[        S5      nXgX4 Vs/ s H  n[        USSXS-  -  S-  5      PM     nn[	        5          [        SXU-  US   S9n[        S	X-  US
   S9n[        SX-  US   S9n[        SX-  US   S9nS S S 5        [        SWWXUR                  US-  UR                  -  U* S-  UR                  -  S9n[        SUWXUR                  US-  UR                  -  U* S-  UR                  -  S9n[        SUWX%UR                  US-  UR                  -  U	* S-  UR                  -  S9nUR                  R                  UR                  5      US-  UR                  -  -   U	S-  UR                  -  -   n[        UR                  UR                  5      UR                  UR                  5      /5      n[	        5          [        UUUU5      nS S S 5        [         R                  n[!        WR"                  U R%                  U5      UR%                  U5      UR%                  U5      /5      nUR%                  U5      R'                  U5      n[)        UR*                  U /U/X/XE/UR"                  UUUR,                  UR.                  S9
nUR1                  5       u  nnU[3        S
5      :X  d   e[        XgXU
/5      n[        XU/5      n[        X4U/5      n [5        UU4UR6                  5      n![5        UU U4UR8                  5      n"[5        UU U4U5      n#/ SQn$/ SQn%/ SQn&[        / SQ/ SQ/ SQ/5      n'[        S/S/S//5      n(Sm)[;        U)4S jU#" U%U&U$5       5       5      (       d   e[;        U)4S j[        U!" U%U$5      5      U'-
   5       5      (       d   e[;        U)4S  j[        U"" U%U&U$5      5      U(-
   5       5      (       d   eg s  snf ! , (       d  f       GNd= f! , (       d  f       GN= f)!Nz
q1:4, u1:4z	l1:5, rhoNr   r[      Link1)r    r7   ry   Link2r   )r7   ry   Link3rZ   Link4rW   )r:   r;   r>   r{   r=   rY   J3)	q_indu_indq_dependentu_dependentkd_eqsconfiguration_constraintsvelocity_constraints	forcelistr!   )gp=
ף?gQ?gzG?g(\?i  )g @gtM)L?gF!8@)g?g68[ƿgൄ?)gDwCA@g_'@gY>)g    gTB?gv1?)g_r=
ףg0 Sg;?gLgd`Egop]?g|=c              3   @   >#    U  H  n[        U5      T:  v   M     g 7frJ   abs.0r?   epss     r1   	<genexpr>@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>   s     F%Es1v|%E   c              3   @   >#    U  H  n[        U5      T:  v   M     g 7frJ   r   r   s     r1   r   r      s       >; !"s1v|;r   c              3   @   >#    U  H  n[        U5      T:  v   M     g 7frJ   r   r   s     r1   r   r      s!      IF !"s1v|Fr   )r   r   r   r   r   r   r	   r@   r?   r#   pos_fromr   dotr   r
   _tr   r&   r'   subsr   r    r"   r!   kanes_equationsr   r   rh   ri   all)*r\   r]   q3r^   r_   u3l1l2l3l4rhor   rC   inertiaslink1link2link3link4joint1joint2joint3loopfhr,   rr   qdotsfhdkanefrfrspr$   r%   eval_meval_feval_fhdp_valsq_valsu_vals
mass_checkforcing_checkr   s*                                            @r1   -test_four_bar_linkage_with_manual_constraintsr      s"   +L9BBB!+.BBCsA>@b=MN=M1aAv!23=MHN		!WA"H%-a[2W38Xa[IW38Xa[IW38Xa[I 
" eU%''!Vegg%B37UWW3DFF eU%''!Vegg%B37UWW3DFF eU%''!Vegg%B37UWW3DFF $$U%5%56!Vegg "Q 01D 
%''"DHHUWW$56	7B		!eVVV< 
" 	A&++
BGGAJ
CDE
''!*//%
 Cv||B4t$&8"$kkR,/6<<$mm	-D
 ""$GBq>> 	$%A|A|Aq!fd../Fq!Qi.FAq	3'H*F9F<F 211	2 3J /0././1 2M CFXfff%EFFFFF >vff-.;> > > > > Ivfff56FI I I I I} O	!	!, 
"	!s    O7AO<)P<
P
PN)+sympy.core.functionr   sympy.core.symbolr   (sympy.functions.elementary.trigonometricr   r   sympy.matrices.denser   sympy.simplify.trigsimpr   sympy.physics.mechanicsr	   r
   r   r   r   r   r   r   r   sympy.physics.vectorr   r   sympy.testing.pytestr   r   sympyr   sympy.utilities.lambdifyr   sympy.solvers.solversr   r   rr   r2   rE   rT   rb   rp   rt   r   r    rP   r1   <module>r      sn    & % ? ' ,. . . @ ?  - ' 2.! <*Z82.!")HDIrP   