§
    OŠtj=  ã                   ód   — 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dS )
é    )ÚzerosÚMatrixÚsymbolsÚlambdifyÚsqrtÚpiÚsimplify)ÚdynamicsymbolsÚcrossÚinertiaÚ	RigidBodyÚReferenceFrameÚKanesMethodc            
      óD  — 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        d	¦  «         |j                             |j        d	¦  «         t          d
¦  «        }| 	                    |j        ||d	fd¦  «         |j         
                    ||j        |¦  «         |j                             |j        ¦  «        }|j                             |j        ||j        z  ||j        z  z   ||j        z  z   ¦  «         |j                             d||j        z  ||j        z  z   ¦  «        }|                     |j        |	|j        z  |
|j        z  z   ¦  «         |j                             |||j        z  ¦  «         |j                             |j        t%          |j                             |j        ¦  «        |j                             |¦  «        ¦  «        ¦  «         |                     |j        ¦  «        |z
  |                     |j        ¦  «        |z
  |                     |j        ¦  «        |z
  |                     | ¦  «        |	z
  |                     | ¦  «        |
z
  g}|                     |j        ¦  «        t%          |j                             |¦  «        |                     |j        ¦  «        ¦  «        z   }|                     |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/html/CA-Chatbot/venv/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_kane5.pyÚ_create_rolling_discrF      sr  € åÔ€AÝ-;¸KÑ-HÔ-HÑ*€BˆˆB��B˜˜B  B¨Ý�gÑÔ�G€A€qˆ!å�xÑ Ô €FÝ�V !Ð$Ñ$Ô$€DØ˜˜Q™‘J ‘N¥W¨T¬Z¸¸A¸qÑ%AÔ%AÑAØ”Oð%€D„Là
Ô×Ò˜fœl¨AÑ.Ô.Ð.Ø„O×Ò˜DœJ¨Ñ*Ô*Ð*Ý˜{Ñ+Ô+€Ià×Ò ¤¨r°2°q¨k¸5ÑAÔAÐAØ„J×Ò˜9 i¤k°2Ñ6Ô6Ð6ØŒJ×!Ò! &¤,Ñ/Ô/€EØ„J×Ò˜6œ<Ø ¤™;¨¨d¬f©Ñ4°r¸D¼F±{ÑBñDô Dð Dð 
Ô	×	$Ò	$ _Ø%'¨&¬(¡]°R¸&¼(±]Ñ%Bñ
Dô 
D€Bà‡J‚JˆvŒ|˜R &¤(™]¨R°&´(©]Ñ:Ñ;Ô;Ð;Ø„O×Ò˜B  I¤K¡Ñ0Ô0Ð0Ø„O×Ò˜FœL­%ØŒ
×Ò˜fœlÑ+Ô+¨T¬_×-EÒ-EÀbÑ-IÔ-Iñ+Kô +Kñ Lô Lð Lð �IŠI�d”fÑÔ Ñ" E§I¢I¨d¬fÑ$5Ô$5¸Ñ$:Ø�IŠI�d”fÑÔ Ñ" B§G¢G¨A¡J¤J°¡O°R·W²W¸Q±Z´ZÀ"±_ðF€Dð 
�Š�”Ñ	Ô	¥ØŒ
×Ò˜iÑ(Ô(¨"¯+ª+°d´oÑ*FÔ*Fñ!Hô !Hñ 
H€Bà�6Š6�&”(ÑÔ˜RŸVšV F¤HÑ-Ô-Ð
.€CàŒo ¤	˜z¨A™~°´Ñ8Ð9Ð:€EØˆV€Fà”Ø�b˜"˜b "Ð%Ø�b˜"�Ø�b�ØØØØð	ð 	ð 	ó    Fc           	      ó.  ‡‡— 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¢}	|	                      |||	|¦  «        d d…df         ¦  «         t          g d¢¦  «        }
t          ˆfd„ |||	|¦  «        |
z
  D ¦   «         ¦  «        sJ ‚d}g d¢}	|	                      |||	|¦  «        d d…df         ¦  «         t          g d¢¦  «        }
t          ˆfd„ |||	|¦  «        |
z
  D ¦   «         ¦  «        sJ ‚|r%d}d}	 |||	|¦  «        t          dd¦  «        k    sJ ‚d S d S )Núq1:6úu1:6r   T)Úcsec                 óJ   •— t          j        d„  ‰| ||¦  «        D ¦   «         Ž S )Nc              3   ó4   K  — | ]}t          |¦  «        V — Œd S ©N)r   )Ú.0Úmats     rE   ú	<genexpr>zE_verify_rolling_disc_numerically.<locals>.<lambda>.<locals>.<genexpr><   s(   è è € Ð	3Ð	3˜#�&�‰+Œ+Ð	3Ð	3Ð	3Ð	3Ð	3Ð	3rG   )r   ÚLUsolve)ÚqÚuÚpÚeval_syss      €rE   ú<lambda>z2_verify_rolling_disc_numerically.<locals>.<lambda>;   s-   ø€ ¥¤Ø	3Ð	3  ¨!¨Q°Ñ!2Ô!2Ð	3Ñ	3Ô	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     V — Œd S rN   ©Úabs©rO   r+   Úepss     €rE   rQ   z3_verify_rolling_disc_numerically.<locals>.<genexpr>I   ó=   øè è € ð ?ð ? �s�1‰vŒv˜Š|ð ?ð ?ð ?ð ?ð ?ð ?rG   )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     V — Œd S rN   r^   r`   s     €rE   rQ   z3_verify_rolling_disc_numerically.<locals>.<genexpr>T   rb   rG   ©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_numericallyrt   7   sa  øø€ Ý˜VÑ$Ô$¥n°VÑ&<Ô&<½gÀgÑ>NÔ>Nˆ!€q€AÝ˜˜A˜q˜	 DÔ$9¸4Ô;LÐ#MØ ð"ñ "ô "€Hð5ð 5ð 5ð 5€Iå˜A˜q  ! œu a˜=¨$¬)µf¸Q¸rÀ¸r¼U±m´mÑ*CÈÐNÑNÔN€KØ
€CØ€Fà*€FØÐÐ€FØ
‡M‚M�+�+˜f f¨fÑ5Ô5°b°q°b¸!°eÔ<Ñ=Ô=Ð=Ýð ð ð ñ ô €Hõ
 ð ?ð ?ð ?ð ?Ø�	˜& &¨&Ñ1Ô1°HÑ<ð?ñ ?ô ?ñ ?ô ?ð ?ð ?ð ?ð -€FØ Ð Ð €FØ
‡M‚M�+�+˜f f¨fÑ5Ô5°b°q°b¸!°eÔ<Ñ=Ô=Ð=Ýð ð ð ñ ô €Hõ
 ð ?ð ?ð ?ð ?Ø�	˜& &¨&Ñ1Ô1°HÑ<ð?ñ ?ô ?ñ ?ô ?ð ?ð ?ð ?àð AØ ˆØ ˆØˆy˜ ¨Ñ0Ô0µE¸"¸a±L´LÒ@Ð@Ð@Ð@ðAð Að AÐ@rG   c                  óú   — t          ¦   «         } t          | d         | d         | d         | d         | d         | d         | d         | d         d	d
¬¦
  «
        }|                     ¦   «          t          |¦  «         d S )Nr   r   r   r   r   r    r!   r"   FÚLU)Úu_dependentÚvelocity_constraintsr!   Ú	forcelistÚexplicit_kinematicsÚconstraint_solver)rF   r   Úkanes_equationsrt   )Úpropsrl   s     rE   Útest_kane_rolling_disc_lur~   \   s„   € Ý Ñ"Ô"€EÝ�u˜W”~ u¨W¤~°u¸W´~Ø˜V”}°%¸´.Ø,1°%¬LØ# Hœo¸¸w¼Ø+0ÀDð	Jñ Jô J€Dð
 	×ÒÑÔÐÝ$ TÑ*Ô*Ð*Ð*Ð*rG   c                  óR  ‡— t          ¦   «         } t          | d         | d         | d         | d         | d         | d         | d         | d         d	d
„ ¬¦
  «
        }t          d¦  «        t          d¦  «        t          d¦  «        }}}t          dd¦  «        }t	          ||||ft          |                     ¦   «                              ¦   «         ¦  «        ¦  «        }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 ‚d S )Nr   r   r   r   r   r    r!   r"   Fc                 óF   — t          |                      |¦  «        ¦  «        S rN   )r	   rR   )ÚAÚbs     rE   rW   z6test_kane_rolling_disc_kdes_callable.<locals>.<lambda>n   s   € ¥8¨A¯IªI°a©L¬LÑ#9Ô#9€ rG   )rw   rx   r!   ry   rz   Úkd_eqs_solverrI   rJ   r   r   rY   )rZ   ç      Ð?g      @re   c              3   óJ   •K  — | ]\  }}t          ||z
  ¦  «        ‰k     V — Œd S rN   r^   ©rO   ÚqdiÚfuira   s      €rE   rQ   z7test_kane_rolling_disc_kdes_callable.<locals>.<genexpr>v   sN   øè è € ð Cð C©¨¨S�s�3˜‘9‰~Œ~ Ò#ð Cð Cð Cð Cð Cð CrG   é   rX   r   gáz®GáÚ?rc   r   gUUUUUUÕ?r„   éþÿÿÿéýÿÿÿc              3   óJ   •K  — | ]\  }}t          ||z
  ¦  «        ‰k     V — Œd S rN   r^   r†   s      €rE   rQ   z7test_kane_rolling_disc_kdes_callable.<locals>.<genexpr>   sF   øè è € ð ;ð ;©¨¨S�s�3˜‘9‰~Œ~ Ò#ð ;ð ;ð ;ð ;ð ;ð ;rG   )rF   r   r
   r   r   ÚtupleÚkindiffdictÚitemsrk   ÚmapÚfloatr   r   )r}   rl   rS   rT   rU   ÚqdÚ	eval_kdesrp   Ú	zero_valsrq   Úqd_valsrr   ra   s               @rE   Ú$test_kane_rolling_disc_kdes_callabler–   g   sv  ø€ Ý Ñ"Ô"€EÝØˆgŒ˜˜gœ¨¨g¬¸¸f¼Ø˜'”N¸¸u¼Ø�XŒ¨%°¬.Ø!Ø9Ð9ð;ñ ;ô ;€Dõ ˜VÑ$Ô$¥n°VÑ&<Ô&<½gÀgÑ>NÔ>Nˆ!€q€AÝ	˜ Ñ	"Ô	"€BÝ˜!˜R  A˜­¨d×.>Ò.>Ñ.@Ô.@×.FÒ.FÑ.HÔ.HÑ(IÔ(IÑJÔJ€IØ
€Cà€FØ€IÝð Cð Cð Cð CØˆy˜ I¨y¸&ÑAÔAðCñ Cô Cñ Cô Cð Cð Cð Cõ •3•u�r A™v¥r¨A¡v­r°A©v°t¸TÐBÑCÔCÑDÔD€FÝ•C�  5¨!¨aµ$°q±'´'©k©/Ø $¨­D°©G¬G©°a©Ñ 8Ø $¨­D°©G¬G©Ñ 4ð 6ñ 7ô 7ñ 8ô 8€Gõ •3•u˜r 1 e¨T°R¸!½dÀ1¹g¼g¹+Ñ5EÑ-FØ#­¨Q©¬ x°!¡|Ñ4ð6ñ 7ô 7ñ 8ô 8€Fåð ;ð ;ð ;ð ;Øˆy˜ ¨&°&Ñ9Ô9ð;ñ ;ô ;ñ ;ô ;ð ;ð ;ð ;ð ;ð ;rG   N)F)Úsympyr   r   r   r   r   r   r	   Úsympy.physics.mechanicsr
   r   r   r   r   r   rF   rt   r~   r–   © rG   rE   ú<module>rš      s  ðð*ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ðBð Bð Bð Bð Bð Bð Bð Bð Bð Bð Bð Bð Bð Bð Bð Bð-ð -ð -ð`"Að "Að "Að "AðJ+ð +ð +ð;ð ;ð ;ð ;ð ;rG   