Ë
    7^(h)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#y)é    )Úsolve)
ÚcosÚexpandÚMatrixÚsinÚsymbolsÚtanÚsqrtÚSÚzerosÚeye)Úsimplify)	ÚdynamicsymbolsÚReferenceFrameÚPointÚ	RigidBodyÚKanesMethodÚinertiaÚParticleÚdotÚfind_dynamicsymbols)Úraisesc            	      ó  ‡‡‡‡‡‡	— t        d«      \  } }}t        d«      \  ŠŠ	‰j                  t        j                  «      ‰	z
  gŠt	        d«      t        d«      cŠ}|j                  ‰d«       t        dt        d«      |«      Š‰j                  j                  || t        ‰«      ‰j                  z  t        ‰«      ‰j                  z  z
  z  «       ‰j                  | |z  ‰j                  z  fŠt        t        ˆˆˆˆˆˆ	fd„«       y )Nzl m gúq uÚNÚOr   ÚPc                  ó.   •— t        ‰‰g‰g‰‰g‰ g¬«      S )N©ÚbodiesÚ	forcelist©r   )ÚFr   r   ÚkdÚqÚus   €€€€€€úe/var/www/skyplay_api_hub/venv/lib/python3.12/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Ü�”e˜C“j !Ó$€AØ‡G�G‡O�O�A�qœC ›F Q§S¡S™L¬3¨q«6°A·C±C©<Ñ7Ñ8Ô9Ø	
�‰�1�"�q‘&˜1Ÿ3™3‘,Ð€AÜ
Œ:÷ :ð :õ ;r)   c                  óX  — t        d«      \  } }t        dd«      \  }}t        d«      \  }}}t        d«      }t        d«      }|j	                  |||j
                  z  «       ||z
  g}	|| | z  ||z  z
  |j
                  z  fg}
t        d||«      }|g}t        || g|g|	«      }|j                  ||
«       |j                  |k(  sJ ‚|j                  |
k(  sJ ‚|j                  }|j                  }|j                  «       |z  }t        |d   «      t        | |z  ||z  z    |z  «      k(  sJ ‚t        |j!                  «       |j"                  j%                  |j&                  «      z
  «      t)        dd«      k(  sJ ‚|j+                  d	¬
«      d   t-        ddg| |z  | |z  gg«      k(  sJ ‚y )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‘S‘Ôà
ˆq‰&ˆ€BØ�ˆr�A‰v˜˜A™‰~ §¡Ñ$Ð
%Ð	&€BÜ	�$˜˜1Ó	€BØ
ˆ€Bä	�Q˜˜˜a˜S "Ó	%€BØ×Ñ�r˜2Ôà�9‰9˜Š?Ðˆ?Ø�8‰8�rŠ>Ðˆ>à	�‰€BØ�j‰j€GØ
�&‰&‹(�WÑ
€CÜ�#�a‘&‹>œV a¨!¡e¨a°!©e¡mÐ$4°qÑ$8Ó9Ò9Ð9Ð9ä�B—F‘F“HØ×'Ñ'×/Ñ/°·±Ó@ñAó BÜEJÈ1ÈaÃ[òQð Qð Qð �L‰L ˆLÓ(¨Ñ+¬v¸¸1°vÀÀÀ1ÁÀqÀbÈÁd¸|Ð6LÓ/MÒMÐNÑMr)   c            	      ó~  ‡‡‡‡‡‡— t        d«      \  ŠŠŠŠt        dd«      \  } }}}t        d«      \  }}}}}t        d«      Št        d«      }	t        d«      }
|	j	                  ‰‰‰j
                  z  «       |
j	                  ‰‰‰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                  }|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 ]W  }||_        t%        |j'                  «       |j(                  j+                  |j,                  «      z
  «      t/        dd«      k(  rŒWJ ‚ | ‰dz  z
  t1        |«      t3        ‰«      z
  gŠt5        t6        ˆˆˆˆˆˆfd„«       y )Núq1 q2 u1 u2r7   úm c1 c2 k1 k2r   ÚP1ÚP2r:   Úpa1Úpa2©Úq_indÚu_indÚkd_eqsr   F)FTé   c                  ó*   •— 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€ 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�N€BˆˆB�Ü'¨°qÓ9Ñ€Cˆˆc�3Ü Ó0Ñ€A€rˆ2ˆr�2Ü�sÓ€AÜ	ˆt‹€BÜ	ˆt‹€BØ‡J�Jˆq�"�q—s‘s‘(ÔØ‡J�Jˆq�2˜‘7˜aŸc™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Ø×Ñ�r˜2ÔØ	�‰€BØ�j‰j€GØ
�&‰&‹(�WÑ
€CÜ�#�a‘&‹>œV b S¨2¡X°°R±Ñ%7¸"¸r¹'Ñ%AÀBÈÁGÑ%KÈQÑ$NÓOÒOÐOÐOÜ�#�a‘&‹>œV R¨"¡W¨r°B©wÑ%6¸¸R¹À"¹Ñ%DÀqØ$&ñH'Ø)+ñH,ñ &,Ø/0ñ%1ó 2ò 2ð 2ð 2ð ×!Ò!Ð!Ð!Ø×Ñ¤ Q£Ò'Ð'Ð'ð #€BÔØ×Ñ¤¬!¨A«$¨q©&°!¨°q¸!°fÐ(=Ó!>Ò>Ð>Ð>ð  -ò UÐØ!4ˆÔÜ˜Ÿ™›Ø×+Ñ+×3Ñ3°B·O±OÓDñEó FÜINÈqÐRSËóUð 	Uð UðUð ��A‘‰+”s˜3“x¤# b£'Ñ)Ð	*€BÜ
Œ:÷ Fð Fõ Gr)   c                  ó(  — t        d«      \  } }t        dd«      \  }}t        d«      \  }}}t        d«      }t        d«      }|j	                  || |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                  }|j                  }|j                  «       |z  }|j                  «        t!        |d   «      t!        | |z  t        | «      z  «      k(  sJ ‚t        |j#                  «       |j$                  j'                  |j(                  «      z
  «      t+        dd«      k(  sJ ‚y )	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¡3Ñ&¨¨Q©´°Q³©¸!¿#¹#Ñ)=Ñ=Ô>Ø
ˆq‰&ˆ€Bàˆa�!‰e�a—c‘c‰kÐ
Ð	€BÜ	�$˜˜1Ó	€BØ
ˆ€Bä	�Q˜˜˜a˜S "Ó	%€BØ×Ñ�r˜2ÔØ	�‰€BØ�j‰j€GØ
�&‰&‹(�WÑ
€CØ‡L�L„NÜ�#�a‘&‹>œV Q B¨¡F¬S°«V¡OÓ4Ò4Ð4Ð4Ü�B—F‘F“HØ×'Ñ'×/Ñ/°·±Ó@ñAó BÜEJÈ1ÈaÃ[òQð Qñ Qr)   c                  ó\  — t        d«      \  } }}}}}t        dd«      \  }}}}	}
}t        d«      \  }}}t        d«      }|j                  dd| |j                  g«      }|j                  dd||j
                  g«      }|j                  dd||j                  g«      }|j                  |«      }|j                  |||j
                  z  ||j                  z  z   ||j                  z  z   «       t        d	«      }|j                  |d
«       |j                  d||j                  z  «      }|j                  |||«       t        ||dz  |dz  z  |dz  |dz  z  |dz  |dz  z  «      }|D �cg c]   }t        |j                  |«      |z
  |«      ‘Œ" }}|| |z  |j                  z  fg}t        d|||||f«      }|g}t!        || ||g|||g|¬«      }|j#                  ||«       |j$                  }|j&                  }|j)                  «       |z  }|j+                  «       } |j-                  | «      }|j/                  «        |j1                  «       t3        d|z  |z  |z  |dz  |z  t5        |«      z  z
  d|z  t7        |«      z  z   d|z  z  d|z  |z  dz  |d|z  |t5        |«      z  z   z  g«      j1                  «       k(  sJ ‚t/        |j9                  «       |j:                  j=                  |j>                  «      z
  «      tA        dd«      k(  sJ ‚|jC                  d¬«      d
   }!|!j-                  |d|d|di«      j-                  | d
|d
|d
|d
|d
i«      }"d
d l"}#|#jG                  |"j-                  |dtI        d«      z  i«      «      jK                  «       tL        jN                  dik(  sJ ‚y c c}w )Núq1 q2 q3 u1 u2 u3r7   úr m gr   ÚYÚAxisÚLÚRÚCr   ÚDmcr[   r:   ÚBodyDrW   é   é   éþÿÿÿé   Tr;   )(r   r   r   Ú	orientnewÚzr/   r0   Ú
ang_vel_inÚset_ang_velr   r,   Ú	locatenewÚv2pt_theoryr   r   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   r   rq   rs   rt   Úw_R_N_qdru   rv   ÚIÚuvr$   Ú	ForceListrw   ÚBodyListrM   rN   r@   rB   ÚkddÚAÚ	A_uprightr„   s$                                       r'   Útest_rolling_discr•   ‹   s§  € ô ,Ð,?Ó@Ñ€BˆˆB��B˜Ü#1Ð2EÀqÓ#IÑ €Cˆˆc�3˜˜SÜ�gÓ�G€A€qˆ!ô 	�sÓ€AØ	�‰�C˜ " a§c¡c Ó+€AØ	�‰�C˜ " a§c¡c Ó+€AØ	�‰�C˜ " a§c¡c Ó+€AØ�|‰|˜A‹€HØ‡M�M�!�R˜!Ÿ#™#‘X  Q§S¡S¡Ñ(¨2°·±©8Ñ3Ô4ô 	ˆc‹
€AØ‡I�Iˆa�„OØ
�+‰+�e˜Q §¡™WÓ
%€CØ‡O�O�A�q˜!Ôô 	��1�q‘5˜1˜a™4‘<  Q¡¨¨A©¡¨q°1©u°q¸!±t©|Ó<€Að 9:Ö	:°"Œ#ˆa�l‰l˜1‹o Ñ(¨"Õ
-Ð	:€BÐ	:ð ˜�s˜Q‘w §¡‘}Ð%Ð&€IÜ�g˜s A q¨1¨c¨(Ó3€EØˆw€Hô 
�Q˜r 2 r˜l°2°r¸2°,ÀrÔ	J€BØ×Ñ�x Ô+Ø	�‰€BØ�j‰j€GØ
�&‰&‹(�WÑ
€CØ
�.‰.Ó
€CØ
�(‰(�3‹-€CØ‡L�L„NØ�:‰:‹<œ6 A b¡D¨¡G¨A¡I°°A±°a±¼¸B»±Ñ$?Ø	ˆ!‰ŒC�‹G‰ñ%Ø˜‘cñ$Ø˜r™E "™H Q™J¨¨B¨r©E°B´s¸2³w±JÑ,>Ñ(?ð#Aó BßBHÁ&Ã(òKð Kð Kä�B—F‘F“HØ×'Ñ'×/Ñ/°·±Ó@ñAó BÜEJÈ1ÈaÃ[òQð Qð Qð 	�‰˜TˆÓ" 1Ñ%€AØ—‘˜˜1˜a  A qÐ)Ó*×/Ñ/°°Q¸¸A¸rÀ1ÀbÈ!ÈRÐQRÐ0SÓT€IÛØ�=‰=˜Ÿ™¨¨Q´°a³©[Ð(9Ó:Ó;×EÑEÓGÌAÏFÉFÐTUÈ;ÒVÐVÑVùòG 
;s   Å)%N)c            	      óP  — t        d«      \  } }}}}}t        dd«      \  }}}}	}
}t        d«      \  }}}}t        dd«      \  }}t        d«      \  }}}t        d«      }|j                  dd| |j                  g«      }|j                  d	d||j
                  g«      }|j                  d
d||j                  g«      }|j                  |«      }|j                  |||j
                  z  ||j                  z  z   ||j                  z  z   «       t        d«      }|j                  |||j
                  z  ||j                  |j
                  z  z  z   «       |j                  d||j                  z  «      }|j                  |||«       |j                  |||«       t        ||dz  |dz  z  |dz  |dz  z  |dz  |dz  z  «      }|D �cg c]   }t        |j                  |«      |z
  |«      ‘Œ" }}|| |z  |j                  z  f|||j
                  z  ||j                  |j
                  z  z  z   fg}t!        d|||||f«      } | g}!t#        || ||g|||||g|¬«      }"|"j%                  |!|«      \  }#}$|#j'                  |d|di«      j'                  |d|di«      }#|$j'                  |d|di«      j'                  |d|di«      }$t#        || ||g|||g|||g¬«      }%|%j%                  |!|«      \  }&}'|&j'                  |d|di«      j'                  |d|di«      }&|'j'                  |d|di«      j'                  |d|di«      }'|$j)                  «        |'j)                  «        |#|&z
  j+                  «       t-        g d¢«      k(  sJ ‚|$|'z
  j+                  «       t-        g d¢«      k(  sJ ‚y c c}w )Nro   r7   zu4, u5, f1, f2zu4, u5rp   r   rq   rr   rs   rt   ru   rv   r[   r:   rw   rW   r   )rX   rY   rZ   Úu_auxiliary)r   r   r   r   r   )r   r   r   r|   r}   r/   r0   r~   r   r   r,   r€   r�   Úa2pt_theoryr   r   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   r   rq   rs   rt   r�   ru   rv   rŽ   r�   r$   r�   rw   r‘   rM   ÚfrÚfrstarÚKM2Úfr2Úfrstar2s(                                           r'   Útest_auxr¤   Õ   sœ  € ô ,Ð,?Ó@Ñ€BˆˆB��B˜Ü#1Ð2EÀqÓ#IÑ €Cˆˆc�3˜˜SÜ#Ð$4Ó5�N€BˆˆB�Ü˜h¨Ó*�H€CˆÜ�gÓ�G€A€qˆ!ä�sÓ€AØ	�‰�C˜ " a§c¡c Ó+€AØ	�‰�C˜ " a§c¡c Ó+€AØ	�‰�C˜ " a§c¡c Ó+€AØ�|‰|˜A‹€HØ‡M�M�!�R˜!Ÿ#™#‘X  Q§S¡S¡Ñ(¨2°·±©8Ñ3Ô4äˆc‹
€AØ‡I�Iˆa��a—c‘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à89Ö	:°"Œ#ˆa�l‰l˜1‹o Ñ(¨"Õ
-Ð	:€BÐ	:à˜�s˜Q‘w §¡‘}Ð%¨¨2°·±©8°b¸A¿C¹CÀ!Ç#Á#¹IÑ6FÑ+FÐ'GÐH€IÜ�g˜s A q¨1¨c¨(Ó3€EØˆw€Hä	�Q˜r 2 r˜l°2°r¸2¸rÀ2Ð2FØô
 €Bà×%Ñ% h°	Ó:�L€RˆØ	�‰�#�q˜#˜qÐ!Ó	"×	'Ñ	'¨¨Q°°A¨Ó	7€BØ�[‰[˜#˜q # qÐ)Ó*×/Ñ/°°Q¸¸A°Ó?€Fä
�a  B¨˜|°B¸¸B°<ÈØ#% r (ô,€Cà×(Ñ(¨°9Ó=�N€Sˆ'Ø
�(‰(�C˜˜C Ð#Ó
$×
)Ñ
)¨2¨q°"°a¨.Ó
9€CØ�l‰l˜C  C¨Ð+Ó,×1Ñ1°2°q¸"¸a°.ÓA€Gà
‡O�OÔØ×ÑÔà�‰H×ÑÓ¤&ªÓ"9Ò9Ð9Ð9Ø�WÑ×$Ñ$Ó&¬&²Ó*AÒAÐAÑAùò- 
;s   Ç%N#c                  óN  — t        d«      } t        d«      \  }}t        d«      \  }}}t        d«      }t        d«      \  }}}	t        d«      \  }
}t        dd«      \  }}t        d«      \  }}t        dd«      \  }}t        d	«      }t        d
«      }|j                  |d| |j                  g«       |j                  || |j                  z  «       t        d«      }|j                  d|
|j                  z  «      }|j                  d||j                  z  «      }|j                  |d«       |j                  |||j                  z  «       |j                  |||«       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!                  ||«      \  }}|j"                  }|d   |	k(  sJ ‚y )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·   	  s4  € ô �c‹l€GÜ�F‹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™c˜
Ô#Ø‡M�M�!�b�S˜1Ÿ3™3‘YÔô 	ˆc‹
€Að 	
�‰�C˜˜aŸc™c™Ó"€AØ	
�‰�T˜1˜qŸs™s™7Ó	#€Bð ‡I�Iˆa�„OØ‡I�Iˆa��a—c‘c‘ÔØ‡N�N�1�a˜ÔÜ�F˜A˜rÓ"€DÜ˜ R¨¨B´¸¸BÀÀBÓ1GÈÐ0KÓL€Hð �b‘˜# ™(Ð#€Hà�hÐ€Hà�q—s‘s�d˜W‘n rÑ)Ð*Ø�a—c‘c�T˜G‘^ bÑ(Ð)Ø�a—c‘c�T˜A‘X  b¡Ñ)Ð*Ø�Q—S‘S˜1‘W�ð€Iô
 
�Q˜˜R˜ 2 r (¨HÓ	5€BØ×%Ñ% h°	Ó:�L€RˆØ	×	Ñ	€BØˆd‰8�rŠ>Ð‰>r)   c                  óÒ  ‡ — t        d«      \  } }t        dd«      \  }}t        d«      \  }}}t        d«      }t        d«      }|j	                  |||j
                  z  «       ||z
  g}	|| | z  ||z  z
  |j
                  z  fg}
t        d||«      }|g}t        || g|g|	«      Š ‰ j                  |«      d   t        dg«      k(  sJ ‚‰ j                  |d ¬«      d   t        dg«      k(  sJ ‚‰ j                  |d ¬	«      d   t        dg«      k(  sJ ‚‰ j                  |«      d   t        dg«      k(  sJ ‚‰ j                  |g «      d   t        dg«      k(  sJ ‚t        t        ˆ fd
„«       t        || g|g|	||
¬«      Š ‰ j                  «       d   t        | |z  || z  z
  g«      k(  sJ ‚t        d«      \  }}}}t        dd«      \  }}}}t        d«      \  }}}}}t        d«      }t        d«      }t        d«      }|j	                  |||j
                  z  «       |j	                  |||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                  }‰ 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 ‚y )Nr   r7   r8   r   r   r9   r   )r    r>   )r>   c                  ó&   •— ‰ j                  d«      S )Nz	bad input)Ú_form_fr)rM   s   €r'   r(   z#test_input_format.<locals>.<lambda>[  s   ø€ ˜rŸ{™{¨;Ó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  sÜ  ø€ ä˜%Ó �D€A€qÜ˜E 1Ó%�F€BˆÜ�gÓ�G€A€qˆ!Ü�sÓ€AÜˆc‹
€AØ‡I�Iˆa��Q—S‘S‘Ôà
ˆq‰&ˆ€BØ�ˆr�A‰v˜˜A™‰~ §¡Ñ$Ð
%Ð	&€BÜ	�$˜˜1Ó	€BØ
ˆ€Bä	�Q˜˜˜a˜S "Ó	%€Bà×Ñ˜bÓ! !Ñ$¬°¨s«Ò3Ð3Ð3à×Ñ R¨tÐÓ4°QÑ7¼6À1À#»;ÒFÐFÐFà×Ñ˜b¨ÐÓ-¨aÑ0´F¸A¸3³KÒ?Ð?Ð?à×Ñ˜bÓ! !Ñ$¬°¨s«Ò3Ð3Ð3à×Ñ˜b "Ó% aÑ(¬F°A°3«KÒ7Ð7Ð7ä
Œ:Ó7Ô8ô 
�Q˜˜˜a˜S "¨R¸2Ô	>€BØ×ÑÓ Ñ"¤f¨q¨b°©d°Q°q±S©j¨\Ó&:Ò:Ð:Ð:ô $ MÓ2�N€BˆˆB�Ü'¨°qÓ9Ñ€Cˆˆc�3Ü Ó0Ñ€A€rˆ2ˆr�2Ü�sÓ€AÜ	ˆt‹€BÜ	ˆt‹€BØ‡J�Jˆq�"�q—s‘s‘(ÔØ‡J�Jˆq�2˜‘7˜aŸc™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ð ×Ñ�r˜2ÔØ	�‰€BØ�j‰j€GØ
�&‰&‹(�WÑ
€CÜ�#�a‘&‹>œV b S¨2¡X°°R±Ñ%7¸"¸r¹'Ñ%AÀBÈÁGÑ%KÈQÑ$NÓOÒOÐOÐOÜ�#�a‘&‹>œV R¨"¡W¨r°B©wÑ%6¸¸R¹À"¹Ñ%DÀqØ$&ñH'Ø)+ñH,ñ &,Ø/0ñ%1ó 2ò 2ð 2ñ 2r)   c                  ól
  — t        d«      } t        d«      }|j                  | d«       t        dd¬«      }| j	                  dd|«      }t        d	«      }|j                  d
|d   |j                  z  |d   |j                  z  z   |d   |j                  z  z   «      }|dd  |z   }|d   g}g }|j                  | «      }	t        d«      \  }
}}|j                  | |
|j                  z  ||j                  z  z   ||j                  z  z   «       |j                  | «      |	z
  j                  «       }|||j                  z  ||j                  z  ||j                  z  gz  }|j                  | «      }t        d«      \  }}}|j                  | ||j                  z  ||j                  z  z   ||j                  z  z   «       |j                  | «      |z
  }|||j                  z  ||j                  z  ||j                  z  gz  }||||
||g}t        |«      }|j                  |z  d   dz
  g}||j                  |j                  «       z  d   gz   }	 t!        | |||||g g g d¬«
      }t%        d«      }t'        |gdD �cg c]   }t)        d|› �«      |d   |d   k(  rdndz  ‘Œ" c}¢­Ž }|j+                  t)        d«      dt)        d«      di«      }t-        d|||||f«      }|g}|j.                  |j0                  t)        d«      z  | j                  z  f|j2                  t        d«      |j                  z  f|j.                  t        d«      |j                  z  fg}|j5                  ||«       t7        |j8                  D �cg c]  }|j;                  «       ‘Œ c}|j<                  D �cg c]  }|j;                  «       ‘Œ c}z   «      }|j>                  } |j@                  }!d|_!        t7        |j8                  D �cg c]  }|j;                  «       ‘Œ c}|j<                  D �cg c]  }|j;                  «       ‘Œ c}z   «      }"|j@                  }#||"z  dk  sJ ‚| |jD                  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<   tG        |d   |d   «      d   }&t        | |$z  «      j+                  |d   |&i«      }'|'|!k(  sJ ‚y # t"        $ r}|‚d }~ww xY wc c}w c c}w c c}w c c}w c c}w )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_B)ÚxxÚyyÚzzÚxyÚyzÚxzÚJ_B_éÿÿÿÿÚJ_B_xyÚJ_B_yzÚRBr4   ÚT_zÚF_zgš™™™™™©?r{   )$r   r   r,   r   r|   r€   r/   r0   r}   r~   r   r   Úvelr   ÚTr*   r   Ú	Exceptionr   r   r   rƒ   r   Ú
masscenterÚmassÚframer=   ÚsumrE   Ú	count_opsrC   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Ç   ÚaxÚJ_BrÒ   Úrigid_bodiesÚ
force_listr/   Ú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ú   ~  st  € ô ˜Ó
€CÜ�'‹N€EØ	‡M�M�#�qÔô ˜<¨dÔ3€EØ�‰�c˜<¨Ó/€Aô ˜7Ó#€EØ�?‰?˜6 5¨¡8¨A¯C©C¡<°%¸±(¸1¿3¹3±,Ñ#>ÀÀqÁÈ!Ï#É#ÁÑ#MÓ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‘sÑØ�q—s‘sÑØ�q—s‘sÑðñ €Mð �x‰x˜‹}€HÜ˜WÓ%�G€A€qˆ!Ø‡L�L��a˜Ÿ™‘e˜a §¡™e‘m a¨¯©¡eÑ+Ô,ð —H‘H˜S“M HÑ,€Lð Ø" Q§S¡SÑ(Ø" Q§S¡SÑ(Ø" Q§S¡SÑ(ðñ €Mð ��1�a˜˜AÐ€Eô �u“€IØ—K‘K 	Ñ)¨1Ñ-°Ñ1Ð2€KØ! i§k¡k°I·N±NÓ4DÑ&DÀaÑ%HÐ$IÑI€MðÜ˜˜e UØØ Ø&1Ø "ØØØ %ô
ˆô �%‹.€CÜ
�!ð =Ø:ö<Øô ˜4 ˜t˜“~¨B¨q©E°R¸±UªN¡qÀÓCò <ò =€Cà
�(‰(”A�h“K ¤A h£K°Ð3Ó
4€CÜ	�4˜˜q #¨¨T {Ó	3€Bà�4€Lð 
�‰˜Ÿ™¤ #£™ s§u¡uÑ,Ð-à	�‰”> %Ó(¨¯©Ñ,Ð-Ø	�‰œ uÓ-¨a¯c©cÑ1Ð2ð€Jð ×Ñ�| ZÔ0ô Ø "§¡Ö0˜1ˆ�‰�Ò0Ø "× 3Ñ 3Ö4˜1ˆ�‰�Ò4ñ	5ó€Nð
  "×1Ñ1ÐØŸ>™>Ðà!€BÔÜØ "§¡Ö0˜1ˆ�‰�Ò0Ø "× 3Ñ 3Ö4˜1ˆ�‰�Ò4ñ	5ó€Nð Ÿ>™>Ðà˜NÑ*¨SÒ0Ð0Ð0ð % r§t¡t§y¡y£{Ñ2Ð5IÑIÌfÐUbÓNcÒ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°qÑ9€LÜÐ5¸ÑFÓG×LÑLÈiÐXYÉlÐ\hÐMiÓj€MØÐ0Ò0Ð0Ñ0øôQ ò Øˆûðüò
<ùò& 	1ùÚ4ùò 	1ùÚ4s6   ÉT
 É7%T
Í:T"Î!T'
Ï3T,ÐT1
Ô
	TÔTÔTc                  ó"  — t        d«      \  } }}}t        d«      \  }}}}}}	t        d«      }
t        d«      }|j                  |
|||fd«       |j	                  |
«      }|j                  |j                  «      |z
  |j                  |j                  «      |z
  |j                  |j                  «      |	z
  g}t        d«      }|j                  |
d«       |j                  d| |j                  z  «      }|j                  |d«       t        d	||«      }t        |
|||g|||	g||g|| | z  |
j                  z  fg¬
«      }|j                  «        ||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&                  «      j)                  ||||||	h«      sJ ‚t+        |j,                  |z
  «      t/        dd«      k(  sJ ‚t+        |j&                  |z
  «      t/        dd«      k(  sJ ‚y )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_fixedr~   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    sk  € ä˜Ó#�J€A€qˆ!ˆQÜ+¨KÓ8Ñ€BˆˆB��B˜Ü�sÓ€AÜ�sÓ€AØ×Ñ˜˜B  B˜<¨Ô/Ø�L‰L˜‹O€Eà�I‰I�a—c‘c‹N˜RÑ §¡¨1¯3©3£°"Ñ!4°e·i±iÀÇÁ³nÀrÑ6IÐJ€DÜˆc‹
€AØ‡I�Iˆa�„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€Dà×ÑÔØ�a˜1‘f‘*œv¢y²)ºYÐ&GÓHÑH€KÜØ	
ˆ1‰ˆa”�R“œ˜R›‘¤3 r£7¬3¨r«7¡?´3°r³7Ñ#:Ñ:Ñ;¸aÀ¹dÀ2¹gÑEÑ	FÐGØ	
ˆˆa�‰c�A�2”s˜2“wœs 2›w‘¬¨R«´°R³©¼¸R»Ñ)@Ñ@ÑAÀAÀbÁDÈÁGÑKÑLÐMðOó P€Kô ˜tŸ|™|Ó,×5Ñ5°r¸2¸rÀ2ÀrÈ2Ð6NÔOÐOÐOÜ�D×$Ñ$ {Ñ2Ó3´u¸QÀ³{ÒBÐBÐBÜ�D—L‘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  © r)   r'   ú<module>r
     sn   ðÝ ÷,÷ ,÷ ,å ,÷?÷ ?õ ?õ (ò;òOò@8GòtQò0GWòT1Bòh6òp:2òzQ1óf?r)   