§
    OŠtj)T  ã                   óÂ   — 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
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 d„ Zd„ Zd„ Zd	„ Zd
„ Zd„ Zd„ Z d„ Z!d„ Z"d„ Z#dS )é    )Úsolve)
ÚcosÚexpandÚMatrixÚsinÚsymbolsÚtanÚsqrtÚSÚzerosÚeye)Úsimplify)	ÚdynamicsymbolsÚReferenceFrameÚPointÚ	RigidBodyÚKanesMethodÚinertiaÚParticleÚdotÚfind_dynamicsymbols)Úraisesc            	      óB  ‡‡‡‡‡‡	— t          d¦  «        \  } }}t          d¦  «        \  ŠŠ	‰                     t          j        ¦  «        ‰	z
  gŠt	          d¦  «        t          d¦  «        cŠ}|                     ‰d¦  «         t          dt          d¦  «        |¦  «        Š‰j         	                    || t          ‰¦  «        ‰j        z  t          ‰¦  «        ‰j        z  z
  z  ¦  «         ‰j        | |z  ‰j        z  fŠt          t          ˆˆˆˆˆˆ	fd„¦  «         d S )Nzl m gúq uÚNÚOr   ÚPc                  ó6   •— t          ‰‰g‰g‰‰g‰ g¬¦  «        S )N©ÚbodiesÚ	forcelist©r   )ÚFr   r   ÚkdÚqÚus   €€€€€€úe/var/www/html/CA-Chatbot/venv/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_kane.pyú<lambda>z*test_invalid_coordinates.<locals>.<lambda>   s+   ø€ �{¨1¨q¨c°A°3¸ÀAÀ3Ø67°Sð :ñ  :ô  :€ ó    )r   Údiffr   Ú_tr   r   Úset_velr   ÚpointÚset_posr   Úxr   Úyr   Ú
ValueError)
ÚlÚmÚgr   r#   r   r   r$   r%   r&   s
       @@@@@@r'   Útest_invalid_coordinatesr5      s"  øøøøøø€ å�gÑÔ�G€A€qˆ!Ý�5‰>Œ>�D€A€qØ
�&Š&•Ô"Ñ
#Ô
# aÑ
'Ð	(€BÝ˜#ÑÔ¥ c¡
¤
€D€A€qØ‡I‚Iˆa��O„O€OÝ�•e˜C‘j”j !Ñ$Ô$€AØ„G‡O‚O�A�q�C ™FœF Q¤S™L­3¨q©6¬6°A´C©<Ñ7Ñ8Ñ9Ô9Ð9Ø	
Œ�1�"�q‘&˜1œ3‘,Ð€AÝ
�:ð :ð :ð :ð :ð :ð :ð :ð :ð :ñ ;ô ;ð ;ð ;ð ;r)   c                  ó°  — t          d¦  «        \  } }t          dd¦  «        \  }}t          d¦  «        \  }}}t          d¦  «        }t          d¦  «        }|                     |||j        z  ¦  «         ||z
  g}	|| | z  ||z  z
  |j        z  fg}
t          d||¦  «        }|g}t          || g|g|	¦  «        }|                     ||
¦  «         |j	        |k    sJ ‚|j
        |
k    sJ ‚|j        }|j        }|                     ¦   «         |z  }t          |d         ¦  «        t          | |z  ||z  z    |z  ¦  «        k    sJ ‚t          |                     ¦   «         |j                             |j        ¦  «        z
  ¦  «        t)          dd¦  «        k    sJ ‚|                     d	¬
¦  «        d         t-          ddg| |z  | |z  gg¦  «        k    sJ ‚d S )Nr   é   úm c kr   r   Úpar   é   T©ÚA_and_B)r   r   r   r   r,   r/   r   r   Úkanes_equationsr    ÚloadsÚmass_matrixÚforcingÚinvr   r   ÚrhsÚmass_matrix_fullÚLUsolveÚforcing_fullr   Ú	linearizer   )r%   r&   ÚqdÚudr3   ÚcÚkr   r   r$   ÚFLr9   ÚBLÚKMÚMMr@   rB   s                    r'   Útest_one_dofrO      sù  € õ ˜%Ñ Ô �D€A€qÝ˜E 1Ñ%Ô%�F€BˆÝ�gÑÔ�G€A€qˆ!Ý�sÑÔ€AÝˆc‰
Œ
€AØ‡I‚Iˆa��Q”S‘ÑÔÐà
ˆq‰&ˆ€BØ�ˆr�A‰v˜˜A™‰~ ¤Ñ$Ð
%Ð	&€BÝ	�$˜˜1Ñ	Ô	€BØ
ˆ€Bå	�Q˜˜˜a˜S "Ñ	%Ô	%€BØ×Ò�r˜2ÑÔÐàŒ9˜Š?ˆ?ˆ?ˆ?ØŒ8�rŠ>ˆ>ˆ>ˆ>à	Œ€BØŒj€GØ
�&Š&‰(Œ(�WÑ
€CÝ�#�a”&‰>Œ>�V a¨!¡e¨a°!©e¡mÐ$4°qÑ$8Ñ9Ô9Ò9Ð9Ð9Ð9å�B—F’F‘H”HØÔ'×/Ò/°´Ñ@Ô@ñAñ Bô BÝEJÈ1ÈaÁ[Ä[òQð Qð Qð Qð �LŠL ˆLÑ(Ô(¨Ô+­v¸¸1°vÀÀÀ1ÁÀqÀbÈÁd¸|Ð6LÑ/MÔ/MÒMÐMÐMÐMÐMÐMr)   c            	      óþ  ‡‡‡‡‡‡— t          d¦  «        \  ŠŠŠŠt          dd¦  «        \  } }}}t          d¦  «        \  }}}}}t          d¦  «        Št          d¦  «        }	t          d¦  «        }
|	                     ‰‰‰j        z  ¦  «         |
                     ‰‰‰z   ‰j        z  ¦  «         | dz  ‰dz  z
  d|z  d‰z  z
  gŠ|	| ‰z  |‰z  z
  |‰z  z   |‰z  z   ‰j        z  f|
| ‰z  |‰z  z
  ‰j        z  fg}t          d|	|¦  «        }t          d	|
|¦  «        }||g}t          ‰‰‰g‰‰g‰¬
¦  «        }|                     ||¦  «         |j	        }|j
        }|                     ¦   «         |z  }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 ‚|j        sJ ‚|j        t          d¦  «        k    sJ ‚d|_        |j        t!          t#          d¦  «        dz  dgddgg¦  «        k    sJ ‚dD ]^}||_        t%          |                     ¦   «         |j                             |j        ¦  «        z
  ¦  «        t/          dd¦  «        k    sJ ‚Œ_| ‰dz  z
  t1          |¦  «        t3          ‰¦  «        z
  gŠt5          t6          ˆˆˆˆˆˆfd„¦  «         d S )Núq1 q2 u1 u2r7   úm c1 c2 k1 k2r   ÚP1ÚP2r:   Úpa1Úpa2©Úq_indÚu_indÚkd_eqsr   F)FTé   c                  ó2   •— t          ‰ ‰‰g‰‰g‰¬¦  «        S )NrW   r"   )r   r$   Úq1Úq2Úu1Úu2s   €€€€€€r'   r(   ztest_two_dof.<locals>.<lambda>p   s*   ø€ �{¨1°R¸°HØ24°b°À"ð Fñ  Fô  F€ r)   )r   r   r   r   r,   r/   r   r   r=   r?   r@   rA   r   Úexplicit_kinematicsÚmass_matrix_kinr   r   r   r   rB   rC   rD   rE   r   r   r   r   r1   )Úq1dÚq2dÚu1dÚu2dr3   Úc1Úc2Úk1Úk2rS   rT   rK   rU   rV   rL   rM   rN   r@   rB   ra   r   r$   r]   r^   r_   r`   s                       @@@@@@r'   Útest_two_dofrk   9   sÉ  øøøøøø€ õ
 $ MÑ2Ô2�N€BˆˆB�Ý'¨°qÑ9Ô9Ñ€Cˆˆc�3Ý Ñ0Ô0Ñ€A€rˆ2ˆr�2Ý�sÑÔ€AÝ	ˆt‰Œ€BÝ	ˆt‰Œ€BØ‡J‚Jˆq�"�q”s‘(ÑÔÐØ‡J‚Jˆq�2˜‘7˜aœc‘/Ñ"Ô"Ð"ð ˆa‰%�"�Q‘$‰,˜˜#™  "¡™Ð	%€Bð ���b‘˜2 ™7Ñ" R¨"¡WÑ,¨r°B©wÑ6¸!¼#Ñ=Ð
>ÀÀrÀcØ
ñGØ�"‰WñGØœñFð Að 
€Bå
�5˜"˜aÑ
 Ô
 €CÝ
�5˜"˜aÑ
 Ô
 €CØ
ˆsˆ€Bõ
 
�Q˜r 2˜h¨r°2¨h¸rÐ	BÑ	BÔ	B€BØ×Ò�r˜2ÑÔÐØ	Œ€BØŒj€GØ
�&Š&‰(Œ(�WÑ
€CÝ�#�a”&‰>Œ>�V b S¨2¡X°°R±Ñ%7¸"¸r¹'Ñ%AÀBÈÁGÑ%KÈQÑ$NÑOÔOÒOÐOÐOÐOÝ�#�a”&‰>Œ>�V R¨"¡W¨r°B©wÑ%6¸¸R¹À"¹Ñ%DÀqØ$&ñH'Ø)+ñH,ñ &,Ø/0ñ%1ñ 2ô 2ò 2ð 2ð 2ð 2ð Ô!Ð!Ð!Ð!ØÔ¥ Q¡¤Ò'Ð'Ð'Ð'ð #€BÔØÔ¥­!¨A©$¬$¨q©&°!¨°q¸!°fÐ(=Ñ!>Ô!>Ò>Ð>Ð>Ð>ð  -ð Uð UÐØ!4ˆÔÝ˜Ÿš™œØÔ+×3Ò3°B´OÑDÔDñEñ Fô FÝINÈqÐRSÉÌòUð Uð Uð Uð Uð
 ��A‘‰+•s˜3‘x”x¥# b¡'¤'Ñ)Ð	*€BÝ
�:ð Fð Fð Fð Fð Fð Fð Fð Fð Fñ Gô Gð Gð Gð Gr)   c                  óŽ  — t          d¦  «        \  } }t          dd¦  «        \  }}t          d¦  «        \  }}}t          d¦  «        }t          d¦  «        }|                     || |z  t          | ¦  «        z  |j        z  ||z  t          | ¦  «        z  |j        z  z   ¦  «         ||z
  g}	|||z  |j        z  fg}
t          d||¦  «        }|g}t          || g|g|	¦  «        }|                     ||
¦  «         |j        }|j        }|                     ¦   «         |z  }|                     ¦   «          t!          |d         ¦  «        t!          | |z  t          | ¦  «        z  ¦  «        k    sJ ‚t          |                     ¦   «         |j                             |j        ¦  «        z
  ¦  «        t+          dd¦  «        k    sJ ‚d S )	Nr   r7   zm l gr   r   r9   r   r:   )r   r   r   r   r,   r   r/   r   r0   r   r   r=   r?   r@   rA   r   r   rB   rC   rD   rE   r   )r%   r&   rG   rH   r3   r2   r4   r   r   r$   rK   r9   rL   rM   rN   r@   rB   s                    r'   Ú	test_pendrm   s   sÁ  € Ý˜%Ñ Ô �D€A€qÝ˜E 1Ñ%Ô%�F€BˆÝ�gÑÔ�G€A€qˆ!Ý�sÑÔ€AÝˆc‰
Œ
€AØ‡I‚Iˆa�!��a‘�#˜a™&œ&‘ 1¤3Ñ&¨¨Q©µ°Q±´©¸!¼#Ñ)=Ñ=Ñ>Ô>Ð>Ø
ˆq‰&ˆ€Bàˆa�!‰e�a”c‰kÐ
Ð	€BÝ	�$˜˜1Ñ	Ô	€BØ
ˆ€Bå	�Q˜˜˜a˜S "Ñ	%Ô	%€BØ×Ò�r˜2ÑÔÐØ	Œ€BØŒj€GØ
�&Š&‰(Œ(�WÑ
€CØ‡L‚L�N„N€NÝ�#�a”&‰>Œ>�V Q B¨¡F­S°©V¬V¡OÑ4Ô4Ò4Ð4Ð4Ð4Ý�B—F’F‘H”HØÔ'×/Ò/°´Ñ@Ô@ñAñ Bô BÝEJÈ1ÈaÁ[Ä[òQð Qð Qð Qð Qð Qr)   c                  óà  ‡ ‡!‡"— t          d¦  «        \  } }}}}}t          dd¦  «        \  }}}}	}
}t          d¦  «        \  }}}t          d¦  «        Š ‰                      dd| ‰ j        g¦  «        }|                     dd||j        g¦  «        }|                     dd||j        g¦  «        Š!‰!                     ‰ ¦  «        Š"‰!                     ‰ ||j        z  ||j        z  z   ||j        z  z   ¦  «         t          d	¦  «        }| 
                    ‰ d
¦  «         |                     d||j        z  ¦  «        }|                     |‰ ‰!¦  «         t          ||dz  |dz  z  |dz  |dz  z  |dz  |dz  z  ¦  «        }ˆ ˆ!ˆ"fd„|D ¦   «         }|| |z  |j        z  fg}t          d|‰!|||f¦  «        }|g}t          ‰ | ||g|||g|¬¦  «        }|                     ||¦  «         |j        }|j        }|                     ¦   «         |z  }|                     ¦   «         }|                     |¦  «        }|                     ¦   «          |                     ¦   «         t1          d|z  |z  |z  |dz  |z  t3          |¦  «        z  z
  d|z  t5          |¦  «        z  z   d|z  z  d|z  |z  dz  |d|z  |t3          |¦  «        z  z   z  g¦  «                             ¦   «         k    sJ ‚t-          |                     ¦   «         |j                             |j        ¦  «        z
  ¦  «        t?          dd¦  «        k    sJ ‚|                      d¬¦  «        d
         }|                     |d|d|di¦  «                             | d
|d
|d
|d
|d
i¦  «        }d
d l!}| "                    |                     |dtG          d¦  «        z  i¦  «        ¦  «         $                    ¦   «         tJ          j&        dik    sJ ‚d S )Núq1 q2 q3 u1 u2 u3r7   úr m gr   ÚYÚAxisÚLÚRÚCr   ÚDmcr[   r:   c                 ó\   •— g | ](}t          ‰                     ‰¦  «        ‰z
  |¦  «        ‘Œ)S © ©r   Ú
ang_vel_in©Ú.0Úuvr   rt   Úw_R_N_qds     €€€r'   ú
<listcomp>z%test_rolling_disc.<locals>.<listcomp>¯   ó2   ø€ Ð	:Ð	:Ð	:°"�#ˆa�lŠl˜1‰oŒo Ñ(¨"Ñ
-Ô
-Ð	:Ð	:Ð	:r)   ÚBodyDrW   é   é   éþÿÿÿé   Tr;   )'r   r   r   Ú	orientnewÚzr/   r0   rz   Úset_ang_velr   r,   Ú	locatenewÚv2pt_theoryr   r   r   r=   r?   r@   rA   ÚkindiffdictÚsubsr   r   r   r	   r   rB   rC   rD   rE   r   rF   ÚsympyÚsympifyr
   Ú	eigenvalsr   ÚZero)#r]   r^   Úq3r_   r`   Úu3rc   rd   Úq3dre   rf   Úu3dÚrr3   r4   rq   rs   ru   rv   ÚIr$   Ú	ForceListr�   ÚBodyListrM   rN   r@   rB   ÚkddÚAÚ	A_uprightr�   r   rt   r~   s#                                   @@@r'   Útest_rolling_discrœ   ‹   s  øøø€ õ ,Ð,?Ñ@Ô@Ñ€BˆˆB��B˜Ý#1Ð2EÀqÑ#IÔ#IÑ €Cˆˆc�3˜˜SÝ�gÑÔ�G€A€qˆ!õ 	�sÑÔ€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ�|Š|˜A‰Œ€HØ‡M‚M�!�R˜!œ#‘X  Q¤S¡Ñ(¨2°´©8Ñ3Ñ4Ô4Ð4õ 	ˆc‰
Œ
€AØ‡I‚Iˆa��O„O€OØ
�+Š+�e˜Q ¤™WÑ
%Ô
%€CØ‡O‚O�A�q˜!ÑÔÐõ 	��1�q‘5˜1˜a™4‘<  Q¡¨¨A©¡¨q°1©u°q¸!±t©|Ñ<Ô<€Að 
;Ð	:Ð	:Ð	:Ð	:Ð	:¸Ð	:Ñ	:Ô	:€Bð ˜�s˜Q‘w ¤‘}Ð%Ð&€IÝ�g˜s A q¨1¨c¨(Ñ3Ô3€EØˆw€Hõ 
�Q˜r 2 r˜l°2°r¸2°,ÀrÐ	JÑ	JÔ	J€BØ×Ò�x Ñ+Ô+Ð+Ø	Œ€BØŒj€GØ
�&Š&‰(Œ(�WÑ
€CØ
�.Š.Ñ
Ô
€CØ
�(Š(�3‰-Œ-€CØ‡L‚L�N„N€NØ�:Š:‰<Œ<�6 A b¡D¨¡G¨A¡I°°A±°a±½¸B¹¼±Ñ$?Ø	ˆ!‰�C�‰GŒG‰ñ%Ø˜‘cñ$Ø˜r™E "™H Q™J¨¨B¨r©E°Bµs¸2±w´w±JÑ,>Ñ(?ð#Añ Bô BßBHÂ&Á(Ä(òKð Kð Kð Kå�B—F’F‘H”HØÔ'×/Ò/°´Ñ@Ô@ñAñ Bô BÝEJÈ1ÈaÁ[Ä[òQð Qð Qð Qð 	�Š˜TˆÑ"Ô" 1Ô%€AØ—’˜˜1˜a  A qÐ)Ñ*Ô*×/Ò/°°Q¸¸A¸rÀ1ÀbÈ!ÈRÐQRÐ0SÑTÔT€IØ€L€L€LØ�=Š=˜Ÿš¨¨Qµ°a±´©[Ð(9Ñ:Ô:Ñ;Ô;×EÒEÑGÔGÍAÌFÐTUÈ;ÒVÐVÐVÐVÐVÐVr)   c            	      ó°  ‡$‡%‡&— t          d¦  «        \  } }}}}}t          dd¦  «        \  }}}}	}
}t          d¦  «        \  }}}}t          dd¦  «        \  }}t          d¦  «        \  }}}t          d¦  «        Š$‰$                     dd| ‰$j        g¦  «        }|                     d	d||j        g¦  «        }|                     d
d||j        g¦  «        Š%‰%                     ‰$¦  «        Š&‰%                     ‰$||j        z  ||j        z  z   ||j        z  z   ¦  «         t          d¦  «        }| 
                    ‰$||j        z  ||j        |j        z  z  z   ¦  «         |                     d||j        z  ¦  «        }|                     |‰$‰%¦  «         |                     |‰$‰%¦  «         t          ||dz  |dz  z  |dz  |dz  z  |dz  |dz  z  ¦  «        }ˆ$ˆ%ˆ&fd„|D ¦   «         }|| |z  |j        z  f|||j        z  ||j        |j        z  z  z   fg}t          d|‰%|||f¦  «        }|g}t!          ‰$| ||g|||||g|¬¦  «        }|                     ||¦  «        \  }} |                     |d|di¦  «                             |d|di¦  «        }|                      |d|di¦  «                             |d|di¦  «        } t!          ‰$| ||g|||g|||g¬¦  «        }!|!                     ||¦  «        \  }"}#|"                     |d|di¦  «                             |d|di¦  «        }"|#                     |d|di¦  «                             |d|di¦  «        }#|                      ¦   «          |#                     ¦   «          ||"z
                       ¦   «         t+          g d¢¦  «        k    sJ ‚| |#z
                       ¦   «         t+          g d¢¦  «        k    sJ ‚d S )Nro   r7   zu4, u5, f1, f2zu4, u5rp   r   rq   rr   rs   rt   ru   rv   r[   r:   c                 ó\   •— g | ](}t          ‰                     ‰¦  «        ‰z
  |¦  «        ‘Œ)S rx   ry   r{   s     €€€r'   r   ztest_aux.<locals>.<listcomp>ð   r€   r)   r�   rW   r   )rX   rY   rZ   Úu_auxiliary)r   r   r   r   r   )r   r   r   r†   r‡   r/   r0   rz   rˆ   r   r,   r‰   rŠ   Úa2pt_theoryr   r   r   r=   rŒ   r   r   r   )'r]   r^   r‘   r_   r`   r’   rc   rd   r“   re   rf   r”   Úu4Úu5Úf1Úf2Úu4dÚu5dr•   r3   r4   rq   rs   ru   rv   r–   r$   r—   r�   r˜   rM   ÚfrÚfrstarÚKM2Úfr2Úfrstar2r   rt   r~   s'                                       @@@r'   Útest_auxr¬   Õ   s  øøø€ õ ,Ð,?Ñ@Ô@Ñ€BˆˆB��B˜Ý#1Ð2EÀqÑ#IÔ#IÑ €Cˆˆc�3˜˜SÝ#Ð$4Ñ5Ô5�N€BˆˆB�Ý˜h¨Ñ*Ô*�H€CˆÝ�gÑÔ�G€A€qˆ!å�sÑÔ€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ�|Š|˜A‰Œ€HØ‡M‚M�!�R˜!œ#‘X  Q¤S¡Ñ(¨2°´©8Ñ3Ñ4Ô4Ð4åˆc‰
Œ
€AØ‡I‚Iˆa��a”c‘˜B !¤#¨¬¡)Ñ,Ñ,Ñ-Ô-Ð-Ø
�+Š+�e˜Q ¤™WÑ
%Ô
%€CØ‡O‚O�A�q˜!ÑÔÐØ‡O‚O�A�q˜!ÑÔÐå��1�q‘5˜1˜a™4‘<  Q¡¨¨A©¡¨q°1©u°q¸!±t©|Ñ<Ô<€Aà	:Ð	:Ð	:Ð	:Ð	:Ð	:¸Ð	:Ñ	:Ô	:€Bà˜�s˜Q‘w ¤‘}Ð%¨¨2°´©8°b¸A¼CÀ!Ä#¹IÑ6FÑ+FÐ'GÐH€IÝ�g˜s A q¨1¨c¨(Ñ3Ô3€EØˆw€Hå	�Q˜r 2 r˜l°2°r¸2¸rÀ2Ð2FØð
 ñ 
 ô 
 €Bà×%Ò% h°	Ñ:Ô:�L€RˆØ	�Š�#�q˜#˜qÐ!Ñ	"Ô	"×	'Ò	'¨¨Q°°A¨Ñ	7Ô	7€BØ�[Š[˜#˜q # qÐ)Ñ*Ô*×/Ò/°°Q¸¸A°Ñ?Ô?€Få
�a  B¨˜|°B¸¸B°<ÈØ#% r (ð,ñ ,ô ,€Cà×(Ò(¨°9Ñ=Ô=�N€Sˆ'Ø
�(Š(�C˜˜C Ð#Ñ
$Ô
$×
)Ò
)¨2¨q°"°a¨.Ñ
9Ô
9€CØ�lŠl˜C  C¨Ð+Ñ,Ô,×1Ò1°2°q¸"¸a°.ÑAÔA€Gà
‡O‚OÑÔÐØ×ÒÑÔÐà�‰H×ÒÑÔ¥&¨¨¨Ñ"9Ô"9Ò9Ð9Ð9Ð9Ø�WÑ×$Ò$Ñ&Ô&­&°°°Ñ*AÔ*AÒAÐAÐAÐAÐAÐAr)   c                  ó´  — t          d¦  «        } t          d¦  «        \  }}t          d¦  «        \  }}}t          d¦  «        }t          d¦  «        \  }}}	t          d¦  «        \  }
}t          dd¦  «        \  }}t          d¦  «        \  }}t          dd¦  «        \  }}t          d	¦  «        }t          d
¦  «        }|                     |d| |j        g¦  «         |                     || |j        z  ¦  «         t          d¦  «        }|                     d|
|j        z  ¦  «        }|                     d||j	        z  ¦  «        }| 
                    |d¦  «         | 
                    |||j        z  ¦  «         |                     |||¦  «         t          d||¦  «        }t          d|||t          ||||	¦  «        |f¦  «        }||z
  ||z
  g}||g}||j	         | z  |z  f||j	         | z  |z  f||j         |z  |
|z
  z  f||j        |z  fg}t          ||
|g||g|¦  «        }|                     ||¦  «        \  }}|j        }|d         |	k    sJ ‚d S )Nr4   zk lsza mA mCr#   zIx Iy Izzq1 q2r7   zu1 u2r   rš   rr   r   ru   ÚAor   ÚCartÚPendulum)r…   r…   )r   r   r   Úorientr‡   rˆ   r   r‰   r/   r0   r,   rŠ   r   r   r   r   r=   rC   ) ÚgravityrJ   ÚlsÚaÚmAÚmCr#   ÚIxÚIyÚIzr]   r^   rc   rd   r_   r`   re   rf   r   rš   r   ru   r®   r¯   r°   ÚkindiffsÚbodyListÚ	forceListÚkmr§   r¨   Úmms                                    r'   Útest_parallel_axisr¿   	  sv  € õ �c‰lŒl€GÝ�F‰OŒO�E€A€rÝ˜	Ñ"Ô"�I€A€rˆ2Ý�sÑÔ€AÝ˜Ñ$Ô$�J€BˆˆBõ ˜GÑ$Ô$�F€BˆÝ˜g qÑ)Ô)�H€CˆÝ˜GÑ$Ô$�F€BˆÝ˜g qÑ)Ô)�H€Cˆõ 	�sÑÔ€AÝ�sÑÔ€Aà‡H‚HˆQ�˜"˜˜aœc˜
Ñ#Ô#Ð#Ø‡M‚M�!�b�S˜1œ3‘YÑÔÐõ 	ˆc‰
Œ
€Að 	
�Š�C˜˜aœc™Ñ"Ô"€AØ	
�Š�T˜1˜qœs™7Ñ	#Ô	#€Bð ‡I‚Iˆa��O„O€OØ‡I‚Iˆa��a”c‘ÑÔÐØ‡N‚N�1�a˜ÑÔÐÝ�F˜A˜rÑ"Ô"€DÝ˜ R¨¨Bµ¸¸BÀÀBÑ1GÔ1GÈÐ0KÑLÔL€Hð �b‘˜# ™(Ð#€Hà�hÐ€Hà�q”s�d˜W‘n rÑ)Ð*Ø�a”c�T˜G‘^ bÑ(Ð)Ø�a”c�T˜A‘X  b¡Ñ)Ð*Ø�Q”S˜1‘W�ð€Iõ
 
�Q˜˜R˜ 2 r (¨HÑ	5Ô	5€BØ×%Ò% h°	Ñ:Ô:�L€RˆØ	Ô	€BØˆdŒ8�rŠ>ˆ>ˆ>ˆ>ˆ>ˆ>r)   c                  óþ  ‡ — t          d¦  «        \  } }t          dd¦  «        \  }}t          d¦  «        \  }}}t          d¦  «        }t          d¦  «        }|                     |||j        z  ¦  «         ||z
  g}	|| | z  ||z  z
  |j        z  fg}
t          d||¦  «        }|g}t          || g|g|	¦  «        Š ‰                      |¦  «        d         t          dg¦  «        k    sJ ‚‰                      |d ¬¦  «        d         t          dg¦  «        k    sJ ‚‰                      |d ¬	¦  «        d         t          dg¦  «        k    sJ ‚‰                      |¦  «        d         t          dg¦  «        k    sJ ‚‰                      |g ¦  «        d         t          dg¦  «        k    sJ ‚t          t          ˆ fd
„¦  «         t          || g|g|	||
¬¦  «        Š ‰                      ¦   «         d         t          | |z  || z  z
  g¦  «        k    sJ ‚t          d¦  «        \  }}}}t          dd¦  «        \  }}}}t          d¦  «        \  }}}}}t          d¦  «        }t          d¦  «        }t          d¦  «        }|                     |||j        z  ¦  «         |                     |||z   |j        z  ¦  «         ||z
  ||z
  g}	|| |z  ||z  z
  ||z  z   ||z  z   |j        z  f|| |z  ||z  z
  |j        z  ff}
t          d||¦  «        }t          d||¦  «        }||f}t          |||g||g|	¬¦  «        Š ‰                      ||
¦  «         ‰ j        }‰ j        }|                     ¦   «         |z  }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 ‚d S )Nr   r7   r8   r   r   r9   r   )r    r>   )r>   c                  ó.   •— ‰                       d¦  «        S )Nz	bad input)Ú_form_fr)rM   s   €r'   r(   z#test_input_format.<locals>.<lambda>[  s   ø€ ˜rŸ{š{¨;Ñ7Ô7€ r)   r   rQ   rR   rS   rT   rU   rV   rW   r:   )r   r   r   r   r,   r/   r   r   r=   r   r   r1   r?   r@   rA   r   )!r%   r&   rG   rH   r3   rI   rJ   r   r   r$   rK   r9   rL   r]   r^   r_   r`   rc   rd   re   rf   rg   rh   ri   rj   rS   rT   rU   rV   rN   r@   rB   rM   s!                                   @r'   Útest_input_formatrÃ   A  sz  ø€ å˜%Ñ Ô �D€A€qÝ˜E 1Ñ%Ô%�F€BˆÝ�gÑÔ�G€A€qˆ!Ý�sÑÔ€AÝˆc‰
Œ
€AØ‡I‚Iˆa��Q”S‘ÑÔÐà
ˆq‰&ˆ€BØ�ˆr�A‰v˜˜A™‰~ ¤Ñ$Ð
%Ð	&€BÝ	�$˜˜1Ñ	Ô	€BØ
ˆ€Bå	�Q˜˜˜a˜S "Ñ	%Ô	%€Bà×Ò˜bÑ!Ô! !Ô$­°¨s©¬Ò3Ð3Ð3Ð3à×Ò R¨tÐÑ4Ô4°QÔ7½6À1À#¹;¼;ÒFÐFÐFÐFà×Ò˜b¨ÐÑ-Ô-¨aÔ0µF¸A¸3±K´KÒ?Ð?Ð?Ð?à×Ò˜bÑ!Ô! !Ô$­°¨s©¬Ò3Ð3Ð3Ð3à×Ò˜b "Ñ%Ô% aÔ(­F°A°3©K¬KÒ7Ð7Ð7Ð7å
�:Ð7Ð7Ð7Ð7Ñ8Ô8Ð8õ 
�Q˜˜˜a˜S "¨R¸2Ð	>Ñ	>Ô	>€BØ×ÒÑÔ Ô"¥f¨q¨b°©d°Q°q±S©j¨\Ñ&:Ô&:Ò:Ð:Ð:Ð:õ $ MÑ2Ô2�N€BˆˆB�Ý'¨°qÑ9Ô9Ñ€Cˆˆc�3Ý Ñ0Ô0Ñ€A€rˆ2ˆr�2Ý�sÑÔ€AÝ	ˆt‰Œ€BÝ	ˆt‰Œ€BØ‡J‚Jˆq�"�q”s‘(ÑÔÐØ‡J‚Jˆq�2˜‘7˜aœc‘/Ñ"Ô"Ð"Ø
�‰(�C˜"‘HÐ	€Bà���b‘˜2 ™7Ñ" R¨"¡WÑ,¨r°B©wÑ6¸!¼#Ñ=Ð
>ÀÀrÀcØ
ñGØ�"‰WñGØœñFð Að 
€Bå
�5˜"˜aÑ
 Ô
 €CÝ
�5˜"˜aÑ
 Ô
 €CØ
ˆsˆ€Bå	�Q˜r 2˜h¨r°2¨h¸rÐ	BÑ	BÔ	B€Bð ×Ò�r˜2ÑÔÐØ	Œ€BØŒj€GØ
�&Š&‰(Œ(�WÑ
€CÝ�#�a”&‰>Œ>�V b S¨2¡X°°R±Ñ%7¸"¸r¹'Ñ%AÀBÈÁGÑ%KÈQÑ$NÑOÔOÒOÐOÐOÐOÝ�#�a”&‰>Œ>�V R¨"¡W¨r°B©wÑ%6¸¸R¹À"¹Ñ%DÀqØ$&ñH'Ø)+ñH,ñ &,Ø/0ñ%1ñ 2ô 2ò 2ð 2ð 2ð 2ð 2ð 2r)   c                  ó¶	  — t          d¦  «        } t          d¦  «        }|                     | d¦  «         t          dd¬¦  «        }|                      dd|¦  «        }t          d	¦  «        }|                     d
|d         |j        z  |d         |j        z  z   |d         |j        z  z   ¦  «        }|dd …         |z   }|d         g}g }| 	                    | ¦  «        }	t          d¦  «        \  }
}}| 
                    | |
|j        z  ||j        z  z   ||j        z  z   ¦  «         | 	                    | ¦  «        |	z
                       ¦   «         }|||j        z  ||j        z  ||j        z  gz  }|                     | ¦  «        }t          d¦  «        \  }}}|                     | ||j        z  ||j        z  z   ||j        z  z   ¦  «         |                     | ¦  «        |z
  }|||j        z  ||j        z  ||j        z  gz  }||||
||g}t          |¦  «        }|j        |z  d         dz
  g}||j        |                     ¦   «         z  d         gz   }	 t!          | |||||g g g d¬¦
  «
        }n# t"          $ r}|‚d }~ww xY wt%          d¦  «        }t'          |gd„ dD ¦   «         ¢R Ž }|                     t+          d¦  «        dt+          d¦  «        di¦  «        }t-          d|||||f¦  «        }|g}|j        |j        t+          d¦  «        z  | j        z  f|j        t          d¦  «        |j        z  f|j        t          d¦  «        |j        z  fg}|                     ||¦  «         t7          d„ |j        D ¦   «         d„ |j        D ¦   «         z   ¦  «        }|j        }|j        }d|_         t7          d„ |j        D ¦   «         d„ |j        D ¦   «         z   ¦  «        } |j        }!|| z  dk     sJ ‚||j!                             ¦   «         z  |z
  t          |¦  «        k    sJ ‚|!}"t          d|
 | | g|
d|| g|| d|
g|||
 dgg¦  «        |z  dz  }#|#d         |"d<   |#d         |"d<   |#d         |"d<   |#d          |"d<   tE          |d         |d         ¦  «        d         }$t          ||"z  ¦  «                             |d         |$i¦  «        }%|%|k    sJ ‚d S )!NÚNEDÚNED_or   z
lambda_0:4T)ÚrealÚBÚ
QuaternionzB_x:zÚB_cmr7   r:   zP Q RzU V WF)Úq_dependentrZ   Úconfiguration_constraintsÚvelocity_constraintsÚu_dependentrŸ   ra   ÚM_Bc                 ó`   — g | ]+}t          d |› �¦  «        |d         |d         k    rdndz  ‘Œ,S )ÚJ_B_r   r7   éÿÿÿÿ)r   )r|   Úaxs     r'   r   z,test_implicit_kinematics.<locals>.<listcomp>Ì  sM   € ð <ð <ð <Øõ ˜ ˜˜‘~”~¨B¨q¬E°R¸´UªN¨N q qÀÑCð <ð <ð <r)   )ÚxxÚyyÚzzÚxyÚyzÚxzÚJ_B_xyÚJ_B_yzÚRBr4   ÚT_zÚF_zc                 ó6   — g | ]}|                      ¦   «         ‘ŒS rx   ©Ú	count_ops©r|   r/   s     r'   r   z,test_implicit_kinematics.<locals>.<listcomp>ß  ó    € Ð0Ð0Ð0˜1ˆ�Š‰ŒÐ0Ð0Ð0r)   c                 ó6   — g | ]}|                      ¦   «         ‘ŒS rx   rà   râ   s     r'   r   z,test_implicit_kinematics.<locals>.<listcomp>à  ó    € Ð4Ð4Ð4˜1ˆ�Š‰ŒÐ4Ð4Ð4r)   c                 ó6   — g | ]}|                      ¦   «         ‘ŒS rx   rà   râ   s     r'   r   z,test_implicit_kinematics.<locals>.<listcomp>è  rã   r)   c                 ó6   — g | ]}|                      ¦   «         ‘ŒS rx   rà   râ   s     r'   r   z,test_implicit_kinematics.<locals>.<listcomp>é  rå   r)   gš™™™™™©?rÒ   r…   )#r   r   r,   r   r†   r‰   r/   r0   r‡   rz   rˆ   r   Úvelr   ÚTr*   r   Ú	Exceptionr   r   rŒ   r   r   Ú
masscenterÚmassÚframer=   ÚsumrE   rC   rb   Úforcing_kinra   r%   r   )&rÅ   rÆ   Úq_attrÈ   Úq_posrÊ   rX   Úq_depÚkinematic_eqsÚ	B_ang_velr   ÚQrt   ÚB_ang_vel_kdÚB_cm_velÚUÚVÚWÚB_ref_vel_kdrY   Ú	q_att_vecÚconfig_consrM   ÚerÏ   ÚJ_BrÜ   Úrigid_bodiesÚ
force_listÚn_ops_implicitÚmass_matrix_kin_implicitÚforcing_kin_implicitÚn_ops_explicitÚforcing_kin_explicitÚqdot_candidateÚquat_dot_textbookÚlambda_0_solÚlhs_candidates&                                         r'   Útest_implicit_kinematicsr  ~  s¢  € õ ˜Ñ
Ô
€CÝ�'‰NŒN€EØ	‡M‚M�#�qÑÔÐõ ˜<¨dÐ3Ñ3Ô3€EØ�Š�c˜<¨Ñ/Ô/€Aõ ˜7Ñ#Ô#€EØ�?Š?˜6 5¨¤8¨A¬C¡<°%¸´(¸1¼3±,Ñ#>ÀÀqÄÈ!Ì#ÁÑ#MÑNÔN€Dà�!�"�"ŒI˜Ñ€EØ�1ŒXˆJ€Eà€Mð —’˜SÑ!Ô!€IÝ˜WÑ%Ô%�G€A€qˆ!Ø‡M‚M�#�q˜œ‘u˜q ¤™u‘} q¨¬¡uÑ,Ñ-Ô-Ð-à—L’L Ñ%Ô%¨	Ñ1×;Ò;Ñ=Ô=€Lð Ø�q”sÑØ�q”sÑØ�q”sÑðñ €Mð �xŠx˜‰}Œ}€HÝ˜WÑ%Ô%�G€A€qˆ!Ø‡L‚L��a˜œ‘e˜a ¤™e‘m a¨¬¡eÑ+Ñ,Ô,Ð,ð —H’H˜S‘M”M HÑ,€Lð Ø" Q¤SÑ(Ø" Q¤SÑ(Ø" Q¤SÑ(ðñ €Mð ��1�a˜˜AÐ€Eõ �u‘”€IØ”K 	Ñ)¨1Ô-°Ñ1Ð2€KØ! i¤k°I·N²NÑ4DÔ4DÑ&DÀaÔ%HÐ$IÑI€MðÝ˜˜e UØØ Ø&1Ø "ØØØ %ð
ñ 
ô 
ˆˆøõ ð ð ð Øˆøøøøðøøøõ �%‰.Œ.€CÝ
�!ð =ð <ð <Ø:ð<ñ <ô <ð =ð =ð =€Cà
�(Š(•A�h‘K”K ¥A h¡K¤K°Ð3Ñ
4Ô
4€CÝ	�4˜˜q #¨¨T {Ñ	3Ô	3€Bà�4€Lð 
Œ˜œ¥ #¡¤™ s¤uÑ,Ð-à	Œ•> %Ñ(Ô(¨¬Ñ,Ð-Ø	Œ� uÑ-Ô-¨a¬cÑ1Ð2ð€Jð ×Ò�| ZÑ0Ô0Ð0õ Ø0Ð0 ¤Ð0Ñ0Ô0Ø4Ð4 Ô 3Ð4Ñ4Ô4ñ	5ñô €Nð
  "Ô1ÐØœ>Ðà!€BÔÝØ0Ð0 ¤Ð0Ñ0Ô0Ø4Ð4 Ô 3Ð4Ñ4Ô4ñ	5ñô €Nð œ>Ðà˜NÑ*¨SÒ0Ð0Ð0Ð0ð % r¤t§y¢y¡{¤{Ñ2Ð5IÑIÍfÐUbÑNcÔNcÒcÐcÐcÐcð
 *€NåØ	
ˆQˆB���Q�BˆØ	
ˆQ��Q�BˆØ	
ˆQˆB��QˆØ	
ˆQ���Qˆð	 ñ ô ð
 
ñð
 ñÐð +¨1Ô-€N�2ÑØ*¨1Ô-€N�1ÑØ*¨1Ô-€N�1ÑØ*¨1Ô-€N�1Ñõ ˜ Qœ¨°1¬Ñ6Ô6°qÔ9€LÝÐ5¸ÑFÑGÔG×LÒLÈiÐXYÌlÐ\hÐMiÑjÔj€MØÐ0Ò0Ð0Ð0Ð0Ð0Ð0s   ÉI0 É0
JÉ:I<É<Jc                  óè  — t          d¦  «        \  } }}}t          d¦  «        \  }}}}}}	t          d¦  «        }
t          d¦  «        }|                     |
|||fd¦  «         |                     |
¦  «        }|                     |j        ¦  «        |z
  |                     |j        ¦  «        |z
  |                     |j        ¦  «        |	z
  g}t          d¦  «        }| 
                    |
d¦  «         |                     d| |j        z  ¦  «        }| 
                    |d¦  «         t          d	||¦  «        }t          |
|||g|||	g||g|| | z  |
j        z  fg¬
¦  «        }|                     ¦   «          ||dz  z  t          g d¢g d¢g d¢g¦  «        z  }t          ||z  | t!          |¦  «        t!          |¦  «        z  t!          |¦  «        t#          |¦  «        z  t#          |¦  «        z  z
  z  ||z  |	z  z
  z  gdg||z  |  t!          |¦  «        t#          |¦  «        z  t!          |¦  «        t!          |¦  «        z  t#          |¦  «        z  z   z  ||z  |z  z   z  gg¦  «        }t%          |j        ¦  «                             ||||||	h¦  «        sJ ‚t+          |j        |z
  ¦  «        t/          dd¦  «        k    sJ ‚t+          |j        |z
  ¦  «        t/          dd¦  «        k    sJ ‚d S )Nzg l m cz	q1:4 u1:4r   rš   Úzxyr   r   ÚPor   r   r:   )r7   r   r   )r   r   r   )r   r   r7   r…   r7   )r   r   r   Úorient_body_fixedrz   r   r/   r0   r‡   r   r,   r‰   r   r   r=   r   r   r   r   r@   Úissubsetr   r?   r   )r4   r2   r3   rI   r]   r^   r‘   r_   r`   r’   r   rš   ÚN_w_AÚkdesr   r  r   ÚkaneÚexpected_mdÚexpected_fds                       r'   Útest_issue_24887r    sÞ  € å˜Ñ#Ô#�J€A€qˆ!ˆQÝ+¨KÑ8Ô8Ñ€BˆˆB��B˜Ý�sÑÔ€AÝ�sÑÔ€AØ×Ò˜˜B  B˜<¨Ñ/Ô/Ð/Ø�LŠL˜‰OŒO€Eà�IŠI�a”c‰NŒN˜RÑ §¢¨1¬3¡¤°"Ñ!4°e·i²iÀÄ±n´nÀrÑ6IÐJ€DÝˆc‰
Œ
€AØ‡I‚Iˆa��O„O€OØ	
�Š�T˜A˜2 ¤™8Ñ	$Ô	$€BØ‡J‚Jˆq�!ÑÔÐÝ��b˜!ÑÔ€AÝ�q˜2˜r 2˜,¨¨R°¨°dÀAÀ3Ø#%¨ r¨A¡v°´¡|Ð"4Ð!5ð7ñ 7ô 7€Dà×ÒÑÔÐØ�a˜1‘f‘*�v y y y°)°)°)¸Y¸Y¸YÐ&GÑHÔHÑH€KÝØ	
ˆ1‰ˆa•�R‘”�˜R™œ‘¥3 r¡7¤7­3¨r©7¬7¡?µ3°r±7´7Ñ#:Ñ:Ñ;¸aÀ¹dÀ2¹gÑEÑ	FÐGØ	
ˆˆa�‰c�A�2•s˜2‘w”w�s 2™wœw‘­¨R©¬µ°R±´©½¸R¹¼Ñ)@Ñ@ÑAÀAÀbÁDÈÁGÑKÑLÐMðOñ Pô P€Kõ ˜tœ|Ñ,Ô,×5Ò5°r¸2¸rÀ2ÀrÈ2Ð6NÑOÔOÐOÐOÐOÝ�DÔ$ {Ñ2Ñ3Ô3µu¸QÀ±{´{ÒBÐBÐBÐBÝ�D”L ;Ñ.Ñ/Ô/µ5¸¸A±;´;Ò>Ð>Ð>Ð>Ð>Ð>r)   N)$r�   r   r   r   r   r   r   r	   r
   r   r   r   Úsympy.simplify.simplifyr   Úsympy.physics.mechanicsr   r   r   r   r   r   r   r   r   Úsympy.testing.pytestr   r5   rO   rk   rm   rœ   r¬   r¿   rÃ   r  r  rx   r)   r'   ú<module>r     sß  ðØ Ð Ð Ð Ð Ð ð,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,ð ,à ,Ð ,Ð ,Ð ,Ð ,Ð ,ð?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð ?ð (Ð 'Ð 'Ð 'Ð 'Ð 'ð;ð ;ð ;ðOð Oð Oð@8Gð 8Gð 8GðtQð Qð Qð0GWð GWð GWðT1Bð 1Bð 1Bðh6ð 6ð 6ðp:2ð :2ð :2ðzQ1ð Q1ð Q1ðf?ð ?ð ?ð ?ð ?r)   