Ë
    7^(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                           úf/var/www/skyplay_api_hub/venv/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Ó�G€A€qˆ!ä�xÓ €FÜ�V !Ô$€DØ˜˜Q™‘J ‘N¤W¨T¯Z©Z¸¸A¸qÓ%AÑAØ—O‘Oð%€D„Là
×Ñ×Ñ˜fŸl™l¨AÔ.Ø‡O�O×Ñ˜DŸJ™J¨Ô*Ü˜{Ó+€Ià×Ñ §¡¨r°2°q¨k¸5ÔAØ‡J�J×Ñ˜9 i§k¡k°2Ô6Ø�J‰J×!Ñ! &§,¡,Ó/€EØ‡J�J×Ñ˜6Ÿ<™<Ø §¡™;¨¨d¯f©f©Ñ4°r¸D¿F¹F±{ÑBôDð 
×	Ñ	×	$Ñ	$ _Ø%'¨&¯(©(¡]°R¸&¿(¹(±]Ñ%Bó
D€Bà‡J�Jˆv�|‰|˜R &§(¡(™]¨R°&·(±(©]Ñ:Ô;Ø‡O�O×Ñ˜B  I§K¡K¡Ô0Ø‡O�O×Ñ˜FŸL™L¬%Ø�
‰
×Ñ˜fŸl™lÓ+¨T¯_©_×-EÑ-EÀbÓ-Ió+Kô Lð �I‰I�d—f‘fÓ Ñ" E§I¡I¨d¯f©fÓ$5¸Ñ$:Ø�I‰I�d—f‘fÓ Ñ" B§G¡G¨A£J°¡O°R·W±W¸Q³ZÀ"±_ðF€Dð 
�‰�—‘Ó	¤Ø�
‰
×Ñ˜iÓ(¨"¯+©+°d·o±oÓ*Fó!Hñ 
H€Bà�6‰6�&—(‘(Ó˜RŸV™V F§H¡HÓ-Ð
.€Cà�o‰o §	¡	˜z¨A™~°·±Ñ8Ð9Ð:€EØˆV€Fà—‘Ø�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˜#Œ&��+Ñ	3ùs   ‚)r   ÚLUsolve)ÚqÚuÚpÚeval_syss      €rE   ú<lambda>z2_verify_rolling_disc_numerically.<locals>.<lambda>;   s!   ø€ ¤§¡Ù	3¡¨!¨Q°Ó!2Ô	3ð!5€ rG   é   ç»½×Ùß|Û=)ç…ëQ¸ž#@g¤p=
×£Ð?gq=
×£p@)g333333Ó?gš™™™™™¹?g…ëQ¸…ÿ?çffffffÖ¿ç)\�Âõ(@)gš™™™™™É¿gÍÌÌÌÌÌô?g333333Ã?r   r   )
gæ¾3ÔŽ4À?g5ðk•¤Ë?g8{‡™ô?g¼a@¢wÔ?güHyR¹?gÉZ—û� í¿g	‘§ÿ‘’¿gy¸øëU@gÑã?ÆU„¿gÛtßÃu#¥?c              3   ó:   •K  — | ]  }t        |«      ‰k  –— Œ y ­wrN   ©Úabs©rO   r+   Úepss     €rE   rQ   z3_verify_rolling_disc_numerically.<locals>.<genexpr>I   ó   øè ø€ ò ? Œs�1‹v˜�|ñ ?ùó   ƒ)gÃõ(\�Â@gìQ¸…ëÑ¿gffffff @r[   r\   )g      Ð¿gš™™™™™Àç×£p=
×ã?)
góÃÆ7³‰š?güWwç˜`å?g¡F-¥îŠÀg[Ã!«Ø?gz¨ãÚ?gâæ»s4÷?gñ4;e¢·†¿g^÷]¶ À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Ó$¤n°VÓ&<¼gÀgÓ>Nˆ!€q€AÜ˜˜A˜q˜	 D×$9Ñ$9¸4×;LÑ;LÐ#MØ ô"€Hó5€Iä˜A˜q  !˜u a˜=¨$¯)©)´f¸Q¸rÀ¸U³mÑ*CÈÔN€KØ
€CØ€Fà*€FÚ€FØ
‡M�M‘+˜f f¨fÓ5°b°q°b¸!°eÑ<Ô=Üò ó €Hô
 ó ?Ù˜& &¨&Ó1°HÑ<ô?ô ?ð ?ð ?ð -€FÚ €FØ
‡M�M‘+˜f f¨fÓ5°b°q°b¸!°eÑ<Ô=Üò ó €Hô
 ó ?Ù˜& &¨&Ó1°HÑ<ô?ô ?ð ?ð ?áØ ˆØ ˆÙ˜ ¨Ó0´E¸"¸a³LÒ@Ð@Ñ@ð 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Ü�u˜W‘~ u¨W¡~°u¸W±~Ø˜V‘}°%¸±.Ø,1°%©LØ# H™o¸¸w¹Ø+0ÀDô	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   € ¤8¨A¯I©I°a«LÓ#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©¨¨SŒs�3˜‘9‹~ Õ#ñ Cùó   ƒ!é   rX   r   gáz®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&   øè ø€ ò ;©¨¨SŒs�3˜‘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Ó$¤n°VÓ&<¼gÀgÓ>Nˆ!€q€AÜ	˜ Ó	"€BÜ˜!˜R  A˜¬¨d×.>Ñ.>Ó.@×.FÑ.FÓ.HÓ(IÓJ€IØ
€Cà€FØ€IÜó CÙ˜ I¨y¸&ÓAôCô Cð Cð Cô ”3”uœr A™v¤r¨A¡v¬r°A©v°t¸TÐBÓCÓD€FÜ”Cœ  5¨!¨a´$°q³'©k©/Ø $¨¬D°«G©°a©Ñ 8Ø $¨¬D°«G©Ñ 4ð 6ó 7ó 8€Gô ”3”u˜r 1 e¨T°R¸!¼dÀ1»g¹+Ñ5EÑ-FØ#¬¨Q« x°!¡|Ñ4ð6ó 7ó 8€Fäó ;Ù˜ ¨&°&Ó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   