
    \h=                     d    S SK JrJrJrJrJrJrJr  S SKJ	r	J
r
JrJrJrJr  S rSS jrS rS rg)	    )zerosMatrixsymbolslambdifysqrtpisimplify)dynamicsymbolscrossinertia	RigidBodyReferenceFrameKanesMethodc            
         [         R                  n [        S5      u
  pp4pVpxp[        S5      u  pn[        S5      n[        SUS9nXS-  -  S-  [	        UR
                  SSS5      -  UR                  4Ul        UR                  R                  UR
                  S	5        UR                  R                  UR
                  S	5        [        S
5      nUR                  UR
                  XS	4S5        UR
                  R                  UUR                  U5        UR
                  R                  UR
                  5      nUR
                  R                  UR
                  XoR                  -  XR                  -  -   XR                  -  -   5        UR                  R!                  SXNR                  -  X^R                  -  -   5      nUR                  UR
                  XR                  -  XR                  -  -   5        UR                  R#                  UUUR                  -  5        UR                  R                  UR
                  [%        UR
                  R                  UR
                  5      UR                  R'                  U5      5      5        UR)                  UR                  5      U-
  UR)                  UR                  5      U-
  UR)                  UR                  5      U-
  UR+                  U 5      U	-
  UR+                  U 5      U
-
  /nUR-                  UR
                  5      [%        UR
                  R                  U5      UR'                  UR                  5      5      -   nUR)                  UR                  5      UR)                  UR                  5      /nUR                  UR.                  * U-  UR                  -  4/nU/nUR
                  XX4U/XgU/X/UUUUS.$ )Nz	q1:6 u1:6g r mgrounddisk)mass         r   	int_framezxycontact_point)frameq_indu_indu_depkdesfnhbodiesloads)r
   _tr   r   r   r   
masscenterset_velr   orient_body_fixedorient_axisy
ang_vel_inset_ang_velxz	locatenewset_posr   pos_fromdotdiffvelr   )tq1q2q3q4q5u1u2u3u4u5grmr   discr   g_w_dcpr   v0r    r"   r!   s                           `/var/www/auris/envauris/lib/python3.13/site-packages/sympy/physics/mechanics/tests/test_kane5.py_create_rolling_discrF      s   A-;K-H*BBBBBgGA!x FV!$DQJNWTZZAq%AAOO%DL
fllA.OODJJ*{+Irqk5AJJ9ikk26JJ!!&,,/EJJ6<<;ff4rFF{BD 
			$	$_%'((]R((]%B
DBJJv||R((]R((]:;OOBIKK0OOFLL%

fll+T__-E-Eb-I+K L IIdff"EIIdff$5$:IIdff"BGGAJORWWQZ"_FD 
	

i("++doo*F!H 
HB66&((RVVFHH-
.Coo		zA~89:EVF""%"	 	    c           	        ^^ [        S5      [        S5      [        S5      pCn[        X#U4U R                  U R                  4SS9mU4S jn[        X#S S U4U R
                  [        US S 5      -  SS9nSmS	nS
n/ SQn	U	R                  U" XU5      S S2S4   5        [        / SQ5      n
[        U4S jU" XU5      U
-
   5       5      (       d   eSn/ SQn	U	R                  U" XU5      S S2S4   5        [        / SQ5      n
[        U4S jU" XU5      U
-
   5       5      (       d   eU(       a  SnSn	U" XU5      [        SS5      :X  d   eg g )Nq1:6u1:6r   T)csec                 F   > [         R                  " S T" XU5       5       6 $ )Nc              3   8   #    U  H  n[        U5      v   M     g 7fN)r   ).0mats     rE   	<genexpr>E_verify_rolling_disc_numerically.<locals>.<lambda>.<locals>.<genexpr><   s     	3!2#&++!2s   )r   LUsolve)qupeval_syss      rE   <lambda>2_verify_rolling_disc_numerically.<locals>.<lambda>;   s    	3!!2	3!5rG      绽|=)Q#@gp=
ף?gq=
ףp@)g333333?g?gQ?ffffffֿ)\(@)gɿg?g333333?r   r   )
g3Ԏ4?g5k?g8{?ga@w?gHyR?gZg	gyU@g?Ugtu#?c              3   @   >#    U  H  n[        U5      T:  v   M     g 7frN   absrO   r+   epss     rE   rQ   3_verify_rolling_disc_numerically.<locals>.<genexpr>I          ?< !"s1v|<   )g(\@gQѿgffffff @r]   r^   )g      пgףp=
?)
g7?gWw`?gF-g[!?gz?gs4?g4;eg^] gw{Dÿg-}¿c              3   @   >#    U  H  n[        U5      T:  v   M     g 7frN   r`   rb   s     rE   rQ   rd   T   re   rf   r   r   r   r   r   
   r   )
r
   r   r   mass_matrix_fullforcing_full_Arsr   extendallr   )kaneall_zerorT   rU   rV   	solve_syssolve_u_depp_valsq_valsu_valsexpectedrc   rW   s              @@rE    _verify_rolling_disc_numericallyrx   7   s   V$nV&<gg>N!Aq	D$9$94;L;L#M "H5IA!ua=$))fQrUm*CNK
CF*FF
MM+ff5bqb!e<=  H
  ?&&1H<? ? ? ? ? -F F
MM+ff5bqb!e<=  H
  ?&&1H<? ? ? ? ?  0E"aL@@@ rG   c                      [        5       n [        U S   U S   U S   U S   U S   U S   U S   U S   S	S
S9
nUR                  5         [        U5        g )Nr   r   r   r   r   r    r!   r"   FLU)u_dependentvelocity_constraintsr!   	forcelistexplicit_kinematicsconstraint_solver)rF   r   kanes_equationsrx   )propsrp   s     rE   test_kane_rolling_disc_lur   \   sc     "EuW~uW~uW~V}%.,1%L#How+0D	JD
 	$T*rG   c                  H  ^ [        5       n [        U S   U S   U S   U S   U S   U S   U S   U S   S	S
 S9
n[        S5      [        S5      [        S5      pCn[        SS5      n[	        X%X44[        UR                  5       R                  5       5      5      nSmSnSn[        U4S jU" XX5       5       5      (       d   e[        [        [        [        S-  [        S-  [        S-  SS45      5      n	[        [        [        SSSS[        S5      -  -
  SS[        S5      -  S-
  -  SS[        S5      -
  -  45      5      n
[        [        [        SSSSSS[        S5      -  -   -  S[        S5      * S-   -  45      5      n[        U4S jU" XX5       5       5      (       d   eg )Nr   r   r   r   r   r    r!   r"   Fc                 6    [        U R                  U5      5      $ rN   )r	   rS   )Abs     rE   rX   6test_kane_rolling_disc_kdes_callable.<locals>.<lambda>n   s    8AIIaL#9rG   )r{   r|   r!   r}   r~   kd_eqs_solverrI   rJ   r   r   r[   )r\         ?g      @ri   c              3   H   >#    U  H  u  p[        X-
  5      T:  v   M     g 7frN   r`   rO   qdifuirc   s      rE   rQ   7test_kane_rolling_disc_kdes_callable.<locals>.<genexpr>v   s&      CA )1s39~#A   "   rZ   r   gzG?rg   r   gUUUUUU?r   c              3   H   >#    U  H  u  p[        X-
  5      T:  v   M     g 7frN   r`   r   s      rE   rQ   r      s%      ;9 )1s39~#9r   )rF   r   r
   r   r   tuplekindiffdictitemsro   mapfloatr   r   )r   rp   rT   rU   rV   qd	eval_kdesrt   	zero_valsru   qd_valsrv   rc   s               @rE   $test_kane_rolling_disc_kdes_callabler   g   s    "Egggf'NuX%.!9;D V$nV&<gg>N!A		"B!d.>.>.@.F.F.H(IJI
CFI CyAC C C C C 3urAvrAvrAvtTBCDFC5!a$q'k/ $DGa 8 $DG 4 6 7 8G 3ur1eTR!d1g+5E-F#Qx!|46 7 8F ;&9; ; ; ; ;rG   N)F)sympyr   r   r   r   r   r   r	   sympy.physics.mechanicsr
   r   r   r   r   r   rF   rx   r   r    rG   rE   <module>r      s7   * * *B B-`"AJ+;rG   