
    sg'                         d dl mZ d dlmZ d dlmZmZ d dlmZ d dl	m
Z
 d dlmZmZmZmZmZmZmZmZmZ d dlmZmZ d dlmZmZ d d	lmZ d d
lmZ d dlm Z  ejB                  Z"d Z#d Z$d Z%d Z&d Z'd Z(d Z)d Z*y)    )expand)symbols)cossin)Matrix)trigsimp)	PinJointJointsMethod	RigidBodyParticleBodyKanesMethodPrismaticJointLagrangesMethodinertia)dynamicsymbolsReferenceFrame)raiseswarns_deprecated_sympy)zeros)lambdify)solvec                     t               5  t        d      } t        d      }d d d        t        d       }t        d      \  }}t	        d      \  }}| j                  || j                  z         t               5  t        | |      }d d d        j                  | j                  k(  sJ |j                  || gk(  sJ |j                  | j                  || j                  j                  z  fgk(  sJ |j                  t        |g      k(  sJ |j                  t        |g      k(  sJ |j                  t        ||j!                         z
  g      k(  sJ |j#                         }|t        | |j!                         z  gg      k(  sJ |j$                  t        |gdgg      k(  sJ |j&                  t        ddgd|gg      k(  sJ t)        |j*                  t,              sJ y # 1 sw Y   xY w# 1 sw Y   kxY w)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	            b/var/www/html/venv/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_jointsmethod.pytest_jointsmethodr2      s   		! II 4A
Cy!HE1,'DAqMM!ACC%		! &a%&<<177"""==QF"""<<Q\\1QWWYY;7888888vqc{"""88vqc{""";;&!affh,0000D6UF1668O,-....&1#s"4444""fq!fq%j-A&BBBBfmm[111' & &s   G$;G1$G.1G;c            
         t        d      \  } }}t        d      }t        d|      }t        d      }t	        d      \  }}t        d||||||  |j                  z  |j                        }t               5  t        ||      }	d d d        	j                  j                  |j                  ||z  |j                  z  f       |	j                          |	j                         }
|
d	   | t        |      z  | z  k(  sJ y # 1 sw Y   {xY w)
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GAq!#A1AY'G% DAqa!gR'))^	=A		! $a#$
LLq1uqss{34

**,Cq6aRAYq[   $ $s   6C==Dc                     t               5  t        d      t        d      } t        d      }d d d        t        d      \  }}t        d |      t	        d| |      t               5  t        t        fd       d d d        t        d| |      t	        d| ||      t               5  t        t        fd	       d d d        t        d| ||      t	        d| |||      t               5  t        t        fd
       d d d        y # 1 sw Y   xY w# 1 sw Y   xY w# 1 sw Y   hxY w# 1 sw Y   y xY w)Nr   r   Tr9   r   P2c                      t               S Nr
   r   r   rH   s   r1   <lambda>z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>C       <2r#:     )r;   c                      t               S rJ   rK   rL   s   r1   rM   z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>H   rN   rO   c                      t               S rJ   rK   rL   s   r1   rM   z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>M   rN   rO   )r   r   r   r	   r   r   
ValueError)r   rG   r$   r%   r   r   rH   s       @@@r1   -test_jointmethod_duplicate_coordinates_speedsrS   :   s(   		! III % DAq	$1a	 B	aA	&B		! <z:;< 
$1Q	'B	a1	-B		! <z:;< 
$1a	#B	aAq	)B		! <z:;< <# < <
< <
< <s/   "D,D#2D/8D;D #D,/D8;Ec                  $   t        d      \  } }t        d      \  }}t        d      \  }}}t               5  t        d      }t        d|      }t        d|      }	d d d        t	        d|| | |j
                  z  |j                  	      }
t	        d
|	||| |	j
                  z  |j                  	      }|j                  ||z  |j
                  z         |	j                  ||z  |j
                  z         t               5  t        ||
|      }d d d        j                          t        |j                        t        g dg dddd|dz  z  |z  t        |      z  d|dz  z  |z  z   |dz  |z  t        |      z  |dz  |z  z   gdd|dz  |z  t        |      z  |dz  |z  z   |dz  |z  gg      k(  sJ t        |j                        t        t        |g|g| |z  |z  t!        | |z         t!        |       z   z  ||z  |z  t!        |       z  z
  |dz  |z  d|z  |z   z  |z  t!        |      z  z   g| |z  |z  t!        | |z         z  |dz  |z  |dz  z  t!        |      z  z
  gg            k(  sJ y # 1 sw Y   xY w# 1 sw Y   uxY w)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PartRrV   rX   r,   s                r1   $test_complete_simple_double_pendulumra   O   s   G$FBG$FBgGAq!		! "ISq!Sq!" 
$5 bjQSS
:B	$uRR bjUWW
>B 
ac!##g	ac!##g		! )aR()
&))*fl6B78!Qq!tVAXc"g=MPQRSUVRVPVWXPX=XZ[]^Z^_`Z`adegahZhklnokopqkqZq6r78!QT!VCG^aQRdSTf=TVWYZVZ[\V\6]6_ /` ` ` ` F''(HVbTB41"Q$q&RUVX[]V]R^adegahRhJi+,Q3q5R=K9;<a461R4"9;Mb;PQTUWQX;XKY JZ,-2a46#b2g,+>AaAcRTgAU+U*V=X 6Y -Z Z Z Z)" ") )s   &I8J8JJc                     t        d      \  } }}}t        d      \  }}}}}t               5  t        d      }	t        d|      }
t        d|      }d d d        t	        d	
| |      }t	        d	|
||      }|	j                  || z  |	j                  z  |

       |	j                  ||z  |	j                  z  |

       |
j                  ||z  |	j                  z  |
       |
j                  ||z  |	j                  z  |
       t               5  t        |	||      }d d d        j                          |j                  }|j                  }|j                  |      }t        |d         t        | | z  ||z  z
  ||z  z   ||z  z   |z        k(  sJ t        |d         t        || z  ||z  z   d|z  |z  z
  d|z  |z  z
  |z        k(  sJ y # 1 sw Y   xY w# 1 sw Y   xY w)Nzq1 q2 u1 u2zm c1 c2 k1 k2WB1r6   B2rV   )r:   r;   rX   )reaction_bodyr   r   rY   )r   r   r   r   r   r   r?   r
   r(   mass_matrixforcingLUsolver   )r[   r\   r]   r^   rD   c1c2k1k2rc   rd   re   rV   rX   r,   MMrh   rB   s                     r1   test_two_dof_jointsro   k   s   #M2NBB0Ar2r2		!  I$Q$Q  
aB	?B	b""R	@BMM"R%)2M.MM"R%)2M.NN2b59BN/NN2b59BN/		! )aR()
			BnnG
**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#   ) )s   &GGGG%c            	      (   t        d      \  } }}t               5  t        d      }t        d|      }d d d        t        d      }t	        d|j                  t              ||  |j                  z  |j                        }| |z  | z  t        |      z  |_
        t               5  t        ||      }d d d        j                  t               |j                         }|d   | t        |      z  | z  k(  sJ y # 1 sw Y   xY w# 1 sw Y   UxY w)	Nr4   r   r5   r6   r$   r   rW   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_pedulumrs      s    gGAq!		! I1 	sAa166!9!R!##X!##	7Aq1s1v-A		! $a#$
_%
**,Cq6aRAYq[    $ $s   C<'D<DDc                  D   t        d      \	  } }}}}}}}}t        d      \  }	}
}}t        d      }t        d      }t               5  t	        d| |t        |||d            }t	        d||t        ||||            }t	        d	      }d d d        t        d
|	|| |j                  z  |j                        }t        d||
|||z
  |j                  z  |j                        }|j                  | |z  |j                  z         |j                  ||z  |j                  z         t               5  t        |||      }d d d        j                          |j                  }|j                  }|j                  |      }d|z  |z  |z  t        |
      z  t!        |
      z  d|z  |z  |z  t        |
      z  t!        |
      z  z   ||z  | z  t        |	      z  z
  ||z  |z  t        |	      z  z
  ||t        |
      dz  z  z   |t!        |
      dz  z  z   |dz  | z  z   |dz  |z  z   z  }|d   |z
  j#                         dk(  sJ ||z
  |dz  z  t        |
      z  t!        |
      z  |z  }|d   |z
  j#                         dk(  sJ y # 1 sw Y   xY w# 1 sw Y   WxY w)Nz)mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, gztheta phi omega alphaABrodr   )r7   r    central_inertiaplater   rV   )r:   r;   r=   r>   rX   )r:   r;   parent_pointr>   rY   r   )r   r   r   r   r   r   r	   r@   r   r   r
   r(   rg   rh   ri   r   r   simplify)mAmBlAlBIAxxIBxxIByyIBzzr/   thetaphiomegaalpharu   rv   rw   ry   r   rV   rX   r,   rn   rh   rB   xds                            r1   test_chaos_pendulumr      s   078c0d-BBD$dA-.EFE3usAsA		! 5r#*1dD!#<>W2Q%,QdD%ACI 
$3E%!cCEEkacc
;B	$UE "R3550SUU
DB OOBqDH	bd133h		! )aR()
			BnnG
**W
C
t)e
e
#c#h
.S
9AHu<Lu<TWZ[^W_<_H= 2v{SZ/023b&2+E
2JKOSVZ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   AJ2JJJc                  Z  ) t        d      \  } }}}}}t        d      \  }}}}	}
t        d      }||||	fD cg c]  }t        |dd|
|dz  z  dz         }}t	               5  t        d||
|z  |d         }t        d	|
|z  |d
         }t        d|
|z  |d         }t        d|
|	z  |d         }d d d        t        d| ||j                  |dz  |j                  z  | dz  |j                  z        }t        d||||j                  |dz  |j                  z  | dz  |j                  z        }t        d||||j                  |dz  |j                  z  |	 dz  |j                  z        }|j                  j                  |j                        |dz  |j                  z  z   |	dz  |j                  z  z   }t        |j                  |j                        |j                  |j                        g      }t	               5  t        ||||      }d d d        t         j                  }t!        j"                  | j%                  |      |j%                  |      |j%                  |      g      }|j%                  |      j'                  |      }t)        |j*                  | g|g||g||g|j"                  |||j,                  |j.                  
      }|j1                         \  }}|t3        d
      k(  sJ t        ||||	|
g      }t        | ||g      }t        |||g      } t5        ||f|j6                        }!t5        || |f|j8                        }"t5        || |f|      }#g d}$g d}%g d}&t        g dg dg dg      }'t        dgdgdgg      }(d)t;        )fd |#|%|&|$      D              sJ t;        )fdt         |!|%|$            |'z
  D              sJ t;        )fd t         |"|%|&|$            |(z
  D              sJ y c c}w # 1 sw Y   axY w# 1 sw Y   	xY w)!Nz
q1:4, u1:4z	l1:5, rhoNr   rZ      Link1)r    r7   rx   Link2r   )r7   rx   Link3rY   Link4rV   )r:   r;   r>   rz   r=   rX   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   :   K   | ]  }t        |      k    y wrJ   abs.0r?   epss     r1   	<genexpr>z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>   s     Fs1v|F   c              3   :   K   | ]  }t        |      k    y wrJ   r   r   s     r1   r   z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>   s      >s1v| >r   c              3   :   K   | ]  }t        |      k    y wrJ   r   r   s     r1   r   z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>   s      Is1v| Ir   )r   r   r   r   r   r   r	   r@   r?   r#   pos_fromr   dotr   r
   _tr   r&   r'   subsr   r    r"   r!   kanes_equationsr   r   rg   rh   all)*r[   r\   q3r]   r^   u3l1l2l3l4rhor   rC   inertiaslink1link2link3link4joint1joint2joint3loopfhr,   rq   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      sO   +L9BBB!+.BBCsA>@"b"=MN1aqAv!23NHN		! JWAC"H%-a[2W38Xa[IW38Xa[IW38Xa[IJ eU2%''!Vegg%B37UWW3DFF eU2%''!Vegg%B37UWW3DFF eU2%''!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"b$kkR,/6<<$mm	-D
 ""$GBq>> 	BB$%AB|AB|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 >vff-.;> > > > Ivfff56FI I I I} OJ J,= =s   P"AP
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   rq   r2   rE   rS   ra   ro   rs   r   r    rO   r1   <module>r      sn    & % ? ' ,. . . @ ?  - ' 2.! <*Z82.!")HDIrO   