
    g!&h=                     b    d dl mZmZmZmZmZmZmZ d dlm	Z	m
Z
mZmZmZmZ d ZddZd Zd Zy)	    )zerosMatrixsymbolslambdifysqrtpisimplify)dynamicsymbolscrossinertia	RigidBodyReferenceFrameKanesMethodc            
         t         j                  } t        d      \
  }}}}}}}}}	}
t        d      \  }}}t        d      }t        d|      }||dz  z  dz  t	        |j
                  ddd      z  |j                  f|_        |j                  j                  |j
                  d	       |j                  j                  |j
                  d	       t        d
      }|j                  |j
                  ||d	fd       |j
                  j                  ||j                  |       |j
                  j                  |j
                        }|j
                  j                  |j
                  ||j                  z  ||j                  z  z   ||j                  z  z          |j                  j!                  d||j                  z  ||j                  z  z         }|j                  |j
                  |	|j                  z  |
|j                  z  z          |j                  j#                  |||j                  z         |j                  j                  |j
                  t%        |j
                  j                  |j
                        |j                  j'                  |                   |j)                  |j                        |z
  |j)                  |j                        |z
  |j)                  |j                        |z
  |j+                  |       |	z
  |j+                  |       |
z
  g}|j-                  |j
                        t%        |j
                  j                  |      |j'                  |j                              z   }|j)                  |j                        |j)                  |j                        g}|j                  |j.                   |z  |j                  z  fg}|g}|j
                  |||||g|||g|	|
g||||d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/pru.catia.catastroantioquia-mas.com/valormas/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_kane5.py_create_rolling_discrF      s<   A-;K-H*BBBBBgGAq!x FV!$DQJNWTZZAq%AAOO%DL
fllA.OODJJ*{+Ir2qk5AJJ9ikk26JJ!!&,,/EJJ6<<;dff4rDFF{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b"b"%b"b	 	    c           	         t        d      t        d      t        d      }}}t        |||f| j                  | j                  fd      fd}t        ||d d |f| j
                  t        |d d       z  d      }dd	}d
}g d}	|	j                   |||	|      d ddf          t        g d      }
t        fd |||	|      |
z
  D              sJ d}g d}	|	j                   |||	|      d ddf          t        g d      }
t        fd |||	|      |
z
  D              sJ |rd}d}	 |||	|      t        dd      k(  sJ y y )Nq1:6u1:6r   T)csec                 F    t        j                  d  | ||      D         S )Nc              3   2   K   | ]  }t        |        y wN)r   ).0mats     rE   	<genexpr>zE_verify_rolling_disc_numerically.<locals>.<lambda>.<locals>.<genexpr><   s     	3#&+	3s   )r   LUsolve)qupeval_syss      rE   <lambda>z2_verify_rolling_disc_numerically.<locals>.<lambda>;   s!    	3!Q!2	3!5 rG      绽|=)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   :   K   | ]  }t        |      k    y wrN   absrO   r+   epss     rE   rQ   z3_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   :   K   | ]  }t        |      k    y wrN   r^   r`   s     rE   rQ   z3_verify_rolling_disc_numerically.<locals>.<genexpr>T   rb   rc   r   r   r   r   r   
   r   )
r
   r   r   mass_matrix_fullforcing_full_Arsr   extendallr   )kaneall_zerorS   rT   rU   	solve_syssolve_u_depp_valsq_valsu_valsexpectedra   rV   s              @@rE    _verify_rolling_disc_numericallyru   7   s   V$nV&<gg>N!qAAq	D$9$94;L;L#M "H5IAq!ua=$))fQrUm*CNK
CF*FF
MM+fff5bqb!e<=  H
  ?&&&1H<? ? ? ? -F F
MM+fff5bqb!e<=  H
  ?&&&1H<? ? ? ?  0E"aL@@@ rG   c                      t               } t        | d   | d   | d   | d   | d   | d   | d   | d   d	d

      }|j                          t        |       y )Nr   r   r   r   r   r    r!   r"   FLU)u_dependentvelocity_constraintsr!   	forcelistexplicit_kinematicsconstraint_solver)rF   r   kanes_equationsru   )propsrm   s     rE   test_kane_rolling_disc_lur   \   sc     "EuW~uW~uW~V}%.,1%L#How+0D	JD
 	$T*rG   c                  D   t               } t        | d   | d   | d   | d   | d   | d   | d   | d   d	d
 
      }t        d      t        d      t        d      }}}t        dd      }t	        ||||ft        |j                         j                                     }dd}d}t        fd |||||      D              sJ t        t        t        t        dz  t        dz  t        dz  ddf            }	t        t        t        ddddt        d      z  z
  ddt        d      z  dz
  z  ddt        d      z
  z  f            }
t        t        t        ddddddt        d      z  z   z  dt        d       dz   z  f            }t        fd ||	|
||      D              sJ y )Nr   r   r   r   r   r    r!   r"   Fc                 6    t        | j                  |            S rN   )r	   rR   )Abs     rE   rW   z6test_kane_rolling_disc_kdes_callable.<locals>.<lambda>n   s    8AIIaL#9 rG   )rx   ry   r!   rz   r{   kd_eqs_solverrI   rJ   r   r   rY   )rZ         ?g      @rf   c              3   F   K   | ]  \  }}t        ||z
        k    y wrN   r^   rO   qdifuira   s      rE   rQ   z7test_kane_rolling_disc_kdes_callable.<locals>.<genexpr>v   s(      CSs39~# C   !   rX   r   gzG?rd   r   gUUUUUU?r   c              3   F   K   | ]  \  }}t        ||z
        k    y wrN   r^   r   s      rE   rQ   z7test_kane_rolling_disc_kdes_callable.<locals>.<genexpr>   s&      ;Ss39~# ;r   )rF   r   r
   r   r   tuplekindiffdictitemsrl   mapfloatr   r   )r~   rm   rS   rT   rU   qd	eval_kdesrq   	zero_valsrr   qd_valsrs   ra   s               @rE   $test_kane_rolling_disc_kdes_callabler   g   s    "Egggf'NuX%.!9;D V$nV&<gg>N!qA		"B!RAd.>.>.@.F.F.H(IJI
CFI CIy&A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   ru   r   r    rG   rE   <module>r      s7   * * *B B-`"AJ+;rG   