Ë
    7^(hÙ3  ã                   óº   — d dl mZmZmZmZmZmZmZ d dlm	Z	 d dl
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mZmZ d dlmZ ed„ «       Zd„ Zd	„ Zd
„ Zd„ Z d„ Z!y)é    )ÚsymbolsÚMatrixÚcosÚsinÚatanÚsqrtÚRational)Úsympify)Úsimplify)Úsolve)ÚdynamicsymbolsÚReferenceFrameÚPointÚdotÚcrossÚinertiaÚKanesMethodÚParticleÚ	RigidBodyÚ
LagrangianÚLagrangesMethod)Úslowc                  ó´  — t        d«      \  } }}}}t        d«      x\  }}}}}	}
}|D �cg c]  }|j                  | «      ‘Œ c}x\  }}}}}}}t        d«      }t        d«      x\  }}}}}}}|D �cg c]  }|j                  | «      ‘Œ c}\  }}}}} }!t        d«      }"t	        d«      }#|"j                  dd||"j                  g«      }$|$j                  d	d||$j                  g«      }%|%j                  d
d||%j                  g«      }&|#j                  d||"j                  z  |	|"j                  z  z   |
|"j                  z  z   «      }'|&j                  |"«      }(|%j                  |"«      })|&j                  |"||%j                  z  ||%j                  z  z   ||%j                  z  z   «       |'j                  |#«      j                  |"«      }*|'j                  |"||&j                  z  ||&j                  z  z   ||&j                  z  z   «       |'j                  d||%j                  z  «      }+|+j                  |'|"|&«       t!        |
t#        |'j                  |+«      |"j                  «      z
  g«      },t!        |&D �-cg c]  }-t#        |+j%                  |"«      |-«      ‘Œ c}-«      }.t!        |%D �-cg c]   }-t#        |(|&j                  |"«      z
  |-«      ‘Œ" c}-|"D �-cg c]   }-t#        |*|'j%                  |"«      z
  |-«      ‘Œ" c}-z   «      }/t'        |/|«      }0|%j                  |"|)j)                  |0«      «       |&j+                  |"|&j                  |"«      j                  |%«      t-        |%j                  |"«      |&j                  |"«      «      z   «       ||z  |$j                  z  }1||dz  z  dz  }2||dz  z  dz  }3t/        |&|2|3|2«      }4t1        d|'|&||4|'f«      }5|5g}6|'|1fg}7t3        |"|||||	g|||g|/|
g|,|||g|.¬«      }8|8j5                  |6|7«      \  }9}:|8j7                  «       };|;j8                  |,k(  sJ ‚|;j:                  |.k(  sJ ‚|;j<                  |.j                  | «      j)                  |8j?                  «       «      k(  sJ ‚t'        |;j@                  |;jB                  z   |«      }<|0jE                  «       D ]  }|<|   |0|   k(  rŒJ ‚ tG        |;jH                  |;jJ                  z   |9z
  |:z
  «      t!        g d¢«      k(  sJ ‚|
| tM        |«      z  i}=|d|tO        |«      |z  |z   |tM        |«      |z  || tO        |«      |z  |z   z  tM        |«      z  |d|| tO        |«      |z  |z   z  tO        |«      z  i}>|d|| tO        |«      |z  |z   z  tM        |«      z  || tO        |«      |z  |z   z  tO        |«      z  |di}?|d|z  tO        |«      z  d|z  z  tO        d|z  «      |dz  z  dz  z   dtM        |«      z  |z  |z  dz  z   |d|d||tO        |«      tO        |«      z  |z  |z  tO        |«      |dz  z  z   z  | |d|z  tO        |«      z  d|z  z  tO        d|z  «      |dz  z  dz  z   dtM        |«      z  |z  |z  dz  z   z  |!| tO        |«      tM        |«      z  |z  |z  tM        |«      |dz  z  z   z  i}@|;jQ                  |=|>|?|@gdd¬«      \  }$}%|d|d|d|d|di}At!        g d¢g d¢g d¢tO        |«      |z  ddddtO        |«       tM        |«       dgtM        |«       |z  ddddtM        |«      tO        |«       dgdtS        dd«      dddddd|z  dz  gg d¢dddddd|z  ddgg«      }Bt!        g «      }C|$j)                  |A«      |Bk(  sJ ‚|%j)                  A«      Ck(  sJ ‚tU        |$j)                  A«      j)                  |dtW        d«      z  «      «      jY                  «       ddik(  sJ ‚|8j7                  d¬ «      };|;jQ                  |=|>|?@gdd¬«      \  }$}%|$j)                  A«      Bk(  sJ ‚|%j)                  A«      Ck(  sJ ‚y c c}w c c}w c c}-w c c}-w c c}-w )!Nz	t r m g vzq1:7zu:6zu1:7ÚNÚNOÚAÚAxisÚBÚCÚCOÚPé   é   ÚDisc)Úkd_eqsÚq_dependentÚconfiguration_constraintsÚu_dependentÚvelocity_constraints)r   r   r   r   é   é   T©Úop_pointÚA_and_Br   é   )r   r   r   r   r   r   r   r/   )r   r   r   r   r   r/   r   r   )r   r   r   r   r   r   r/   r   )r   r   r   r   r   r   r   r   éþÿÿÿé   é   ÚGJ)Úlinear_solver)-r   r   Údiffr   r   Ú	orientnewÚzÚxÚyÚ	locatenewÚ
ang_vel_inÚset_ang_velÚpos_fromÚdtÚset_velÚv2pt_theoryr   r   Úvelr   ÚsubsÚset_ang_accr   r   r   r   Úkanes_equationsÚto_linearizerÚf_cÚf_vÚf_aÚkindiffdictÚf_0Úf_1Úkeysr   Úf_2Úf_3r   r   Ú	linearizer	   r
   r   Ú	eigenvals)DÚtÚrÚmÚgÚvÚq1Úq2Úq3Úq4Úq5Úq6ÚqÚqiÚq1dÚq2dÚq3dÚq4dÚq5dÚq6dÚqdÚuÚu1Úu2Úu3Úu4Úu5Úu6ÚuiÚu1dÚu2dÚu3dÚu4dÚu5dÚu6dr   r   r   r   r   r    Úw_c_n_qdÚw_b_n_qdÚ	v_co_n_qdr!   rF   ÚuvrG   ÚkindiffsÚqdotsÚF_COÚIÚJÚI_C_COr$   ÚBLÚFLÚKMÚfrÚfr_starÚ
linearizerÚsolÚq_opÚu_opÚqd_opÚud_opÚupright_nominalÚA_solÚB_solsD                                                                       új/var/www/skyplay_api_hub/venv/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_linearize.pyÚ test_linearize_rolling_disc_kanerŒ      s„  € ô ˜KÓ(�M€A€qˆ!ˆQ�ô "0°Ó!7Ð7Ñ€BˆˆB��B˜˜QØ>?Ö(@¸¨¯©°­Ò(@Ð@Ñ €Cˆˆc�3˜˜S 2ô 	�uÓ€AÜ!/°Ó!7Ð7Ñ€BˆˆB��B˜˜QØ9:Ö#;°2 B§G¡G¨A¥JÒ#;Ñ €Cˆˆc�3˜˜Sô 	�sÓ€AÜ	ˆt‹€BØ	�‰�C˜ " a§c¡c Ó+€AØ	�‰�C˜ " a§c¡c Ó+€AØ	�‰�C˜ " a§c¡c Ó+€AØ	�‰�d˜B˜qŸs™s™F R¨¯©¡V™O¨b°·±©fÑ4Ó	5€Bð �|‰|˜A‹€HØ�|‰|˜A‹€Hð ‡M�M�!�R˜Ÿ™‘V˜b §¡™f‘_ r¨!¯#©#¡vÑ-Ô.ð —‘˜B“×"Ñ" 1Ó%€Ið ‡J�Jˆq�"�Q—S‘S‘&˜2˜aŸc™c™6‘/ B q§s¡s¡FÑ*Ô+ð 	�‰�S˜!˜AŸC™C™%Ó €AØ‡M�M�"�a˜Ôô �"”s˜2Ÿ;™; q›>¨1¯3©3Ó/Ñ/Ð0Ó
1€Cô ¨aÖ0¨”#�a—e‘e˜A“h Õ#Ò0Ó
1€Cô ÀQÖG¸r”s˜8 a§l¡l°1£oÑ5°rÕ:ÒGØBCÖD¸Bœ˜Y¨¯©°«Ñ2°BÕ7ÒDñEó F€Hä�(˜BÓ€Eð ‡M�M�!�X—]‘] 5Ó)Ô*Ø‡M�M�!�Q—\‘\ !“_×'Ñ'¨Ó*¬U°1·<±<À³?ÀAÇLÁLÐQRÃOÓ-TÑTÔUð ˆQ‰3ˆq�s‰s‰7€Dð 
ˆQ�‰T‰�Q‰€AØ	
ˆQ�‰T‰�Q‰€AÜ�Q˜˜1˜aÓ €Fä�V˜R  A¨° |Ó4€DØ
ˆ€BØˆtˆ*ˆ€BÜ	�Q˜˜R  R¨Ð,¨r°2°r¨lÀ8Ø˜¸Ø˜R ˜¸3ô
@€Bð ×&Ñ& r¨2Ó.�M€Rˆð ×!Ñ!Ó#€JØ�>‰>˜SÒ Ð Ð Ø�>‰>˜SÒ Ð Ð Ø�>‰>˜SŸX™X a›[×-Ñ-¨b¯n©nÓ.>Ó?Ò?Ð?Ð?Ü
�
—‘ §¡Ñ/°Ó
4€CØ�j‰j‹lò $ˆØ�2‰w˜% ™)Ó#Ð#Ð#ð$ä�J—N‘N Z§^¡^Ñ3°bÑ8¸7ÑBÓCÄvÊiÓGXÒXÐXÐXð ��”3�r“7‘
Ð€DØ�Ø”�B“˜‘˜cÑ!Ø”�B“˜‘Ø��”C˜“G˜C‘K #Ñ%Ñ&¤s¨2£wÑ.Ø�Ø��”C˜“G˜C‘K #Ñ%Ñ&¤s¨2£wÑ.ð0€Dð �!Ø�1�"”c˜"“g˜c‘k CÑ'Ñ(¬¨R«Ñ0Ø�1�"”c˜"“g˜c‘k CÑ'Ñ(¬¨R«Ñ0Ø�!ð€Eð �!�A‘#”c˜"“g‘+˜q ™sÑ#¤c¨!¨B©$£i°°Q±Ñ&6°qÑ&8Ñ8¸1¼SÀ»W¹9ÀS¹=ÈÑ;LÈQÑ;NÑNØ�!Ø�!Ø�!”S˜“WœS ›W‘_ SÑ(¨Ñ,¬s°2«w°s¸A±v©~Ñ=Ñ>Ø�!�Q�q‘Sœ˜R›‘[ ! A¡#Ñ&¬¨Q¨r©T«°3¸±6Ñ)9¸!Ñ);Ñ;¸aÄÀBÃ¹iÈ¹mÈCÑ>OÐPQÑ>QÑQÑRØ�1�"”c˜"“gœc "›g‘o cÑ)¨#Ñ-´°B³¸¸Q¹±Ñ>Ñ?ðA€Eð ×Ñ¨$°°e¸UÐ)CÈTÐ\`ÐÓa�D€A€qà˜A˜r 1 a¨¨A¨q°!°QÐ7€Oô Ò,Ú,Ú,Ü˜“W˜S‘[ ! Q¨¨1¬s°2«w¨h¼¸R»¸À!ÐDÜ˜"“g�X˜c‘\ 1 a¨¨A¬s°2«w¼¸R»¸À!ÐDØœ  A›¨¨1¨a°°A°q¸±u¸Q±wÐ?Ú,Ø˜˜1˜a  B s¡F¨A¨qÐ1ð3ó 4€Eô �2‹J€Eð �6‰6�/Ó" eÒ+Ð+Ð+Ø�6‰6�/Ó" eÒ+Ð+Ð+ô �1—6‘6˜/Ó*×/Ñ/°°Q´t¸A³w±YÓ?Ó@×JÑJÓLÐQRÐTUÐPVÒVÐVÐVð ×!Ñ!°Ð!Ó5€JØ×Ñ¨$°°e¸UÐ)CØ(,°tð  ó =�D€A€qà�6‰6�/Ó" eÒ+Ð+Ð+Ø�6‰6�/Ó" eÒ+Ð+Ñ+ùòs )Aùò
 $<ùò> 1ùò HùÚDs   ªaÁ1aÉ2"aÊ$%aË%a
c                  óâ  — t        d«      } t        d«      }t        dd«      }t        d«      \  }}}d}t        d«      }t        d«      }|j	                  |d«       |j                  d	d
| |j                  g«      }	|	j                  |||j                  z  «       |j                  d||	j                  z  «      }
|
j                  |||	«       t        d|
|«      }t        ||z
  g«      }||z  |j                  z  }t        || g|g|¬«      }|j                  |g|
|fg«      \  }}|j                  dd¬«      \  }	}}|	t        ddgdt!        | «      z  |z  dgg«      k(  sJ ‚|t        g «      k(  sJ ‚y )NrV   rf   r/   úL, m, tçš™™™™™#@r   úN*r   r   Úaxisr!   ÚpP)Úq_indÚu_indr%   T)r.   r   çš™™™™™#À)r   r   r   r   r?   r6   r7   r<   r:   r8   r@   r   r   r   rD   rO   r   )rV   rf   r^   ÚLrS   rQ   rT   r   ÚpNr   r!   r’   ÚkdeÚRr   r€   Úfrstarr   Úinp_vecs                      r‹   Ú$test_linearize_pendulum_kane_minimalrœ   �   st  € Ü	˜Ó	€BÜ	˜Ó	€BÜ
˜˜qÓ
!€CÜ�iÓ �G€A€qˆ!Ø€Aô 	�sÓ€AÜ	ˆt‹€BØ‡J�Jˆq�!Ôð 	
�‰�C˜ " a§c¡c Ó+€AØ‡M�M�!�R˜Ÿ™‘VÔð 	�‰�S˜!˜AŸC™C™%Ó €AØ‡M�M�"�a˜ÔÜ	�$˜˜1Ó	€Bô �#˜‘(�Ó
€Cð 	
ˆ!‰ˆA�C‰C‰€Aô 
�Q˜r˜d¨2¨$°sÔ	;€BØ×%Ñ% r d¨a°¨V¨HÓ5�L€Rˆð —L‘L¨¸�LÓ=�M€A€qˆ'à”˜˜A˜ ¤c¨"£g¡¨a¡°Ð 3Ð4Ó5Ò5Ð5Ð5Ø”�r“
Š?Ð‰?ó    c                  ó\  — t        d«      \  } }t        dd¬«      \  }}t        d«      \  }}t        dd¬«      \  }}t        d«      \  }}	}
d}t        d«      }t        d«      }|j	                  |d	«       t        || z  «      }|j                  d
d||j                  g«      }|j                  d| |j                  z  ||j                  z  z   «      }t        d||	«      }t        ||z
  ||z
  g«      }t        |||g«      }|j	                  ||j                  |«      j                  |«      j!                  |«      «       t        |j                  |«      j#                  «       |z
  g«      }t        |j%                  |«      j'                  |«      j)                  |j                  «      g«      }|j+                  «        |j-                  |
«      }|j+                  «        |	|z  |j                  z  }t/        ||g|g| g|g||||¬«	      }|j1                  |g||fg«      \  }}| ||d	i}|d	|d	i}|d	|d	i}|j3                  |||gdd¬«      \  }}}|j5                  «       t        d	dgd|z  d	gg«      k(  sJ ‚|t        g «      k(  sJ ‚|j3                  |||gddd¬«      \  }}}|j5                  «       t        d	dgd|z  d	gg«      k(  sJ ‚|t        g «      k(  sJ ‚|j3                  |||gddd„ ¬«      \  }}}|j5                  «       t        d	dgd|z  d	gg«      k(  sJ ‚|t        g «      k(  sJ ‚y )Núq1:3r/   ©Úlevelzu1:3rŽ   r�   r   r�   r   r   r‘   ÚP1r’   )r“   r”   r&   r(   r'   r)   Úacceleration_constraintsr%   Tr,   r•   r3   )r-   r.   r   r4   c                 ó$   — | j                  |«      S ©N©ÚLUsolve©r   Úbs     r‹   ú<lambda>z9test_linearize_pendulum_kane_nonminimal.<locals>.<lambda>ý   s   € ¸A¿I¹IÀa»L€ r�   )r   r   r   r   r?   r   r6   r7   r:   r8   r9   r   r   r   r=   r>   rB   Ú	magnituderA   Úexpressr   r   r5   r   rD   rO   Úexpand) rV   rW   r^   r_   rf   rg   rm   rn   r–   rS   rQ   rT   r   r—   Útheta1r   r!   r’   r˜   Údq_dictrF   rG   rH   r™   r   r€   rš   r„   r…   r‡   r   r›   s                                    r‹   Ú'test_linearize_pendulum_kane_nonminimalr°   ²   sa  € ô ˜FÓ#�F€BˆÜ˜f¨AÔ.�H€CˆÜ˜FÓ#�F€BˆÜ˜f¨AÔ.�H€CˆÜ�iÓ �G€A€qˆ!Ø€Aô 	�sÓ€AÜ	ˆt‹€BØ‡J�Jˆq�!Ôô �"�R‘%‹[€FØ	�‰�C˜ &¨!¯#©# Ó/€Að 	�‰�T˜2˜aŸc™c™6 B q§s¡s¡F™?Ó+€AÜ	�$˜˜1Ó	€Bô �#˜‘(Ø˜‘(ðó €Cä�C˜#˜s˜Ó$€Gð ‡I�Iˆa�—‘˜B“×"Ñ" 1Ó%×*Ñ*¨7Ó3Ô4ô �!—*‘*˜R“.×*Ñ*Ó,¨qÑ0Ð1Ó
2€Cô �!—%‘%˜“(×"Ñ" 1Ó%×)Ñ)¨!¯#©#Ó.Ð/Ó
0€CØ‡L�L„Nð �(‰(�1‹+€CØ‡L�L„Nð 	
ˆ!‰ˆA�C‰C‰€Aô 
�Q˜r˜d¨2¨$¸R¸DØ˜¸Ø!$¸sÈ3ô
P€Bð ×%Ñ% r d¨a°¨V¨HÓ5�L€Rˆð ��2�qˆ>€DØ��2�qˆ>€DØ�!�S˜!Ð€Eà—L‘L¨4°°uÐ*=ÀtØ*.ð !ó 0�M€A€qˆ'ð �8‰8‹:œ ! Q ¨$¨q©&°!¨Ð 5Ó6Ò6Ð6Ð6Ø”�r“
Š?Ðˆ?ð —L‘L¨4°°uÐ*=ÀtØ)-¸Tð !ó C�M€A€qˆ'ð �8‰8‹:œ ! Q ¨$¨q©&°!¨Ð 5Ó6Ò6Ð6Ð6Ø”�r“
Š?Ðˆ?à—L‘L¨4°°uÐ*=Ø)-Ø*.Ù/Hð !ó J�M€A€qˆ'ð
 �8‰8‹:œ ! Q ¨$¨q©&°!¨Ð 5Ó6Ò6Ð6Ð6Ø”�r“
Š?Ð‰?r�   c                  ó~  — t        d«      } t        dd«      }t        d«      \  }}}d}t        d«      }t        d«      }|j	                  |d«       |j                  dd	| |j                  g«      }|j                  |||j                  z  «       |j                  d
||j                  z  «      }	|	j                  |||«       t        d|	|«      }
t        ||
«      }t        || g|	||z  |j                  z  fg|¬«      }|j                  «        |j                  | g|gd¬«      \  }}}t!        |«      t#        ddgdt%        | «      z  |z  dgg«      k(  sJ ‚|t#        g «      k(  sJ ‚|j                  | g|gdd¬«      \  }}}t!        |«      t#        ddgdt%        | «      z  |z  dgg«      k(  sJ ‚|t#        g «      k(  sJ ‚y )NrV   r/   rŽ   r�   r   r�   r   r   r‘   r!   r’   )Ú	forcelistÚframeT)r.   r•   r3   )r.   r4   )r   r   r   r   r?   r6   r7   r<   r:   r8   r@   r   r   r   Úform_lagranges_equationsrO   r   r   r   )rV   r^   r–   rS   rQ   rT   r   r—   r   r!   r’   ÚLagÚLMr   r›   s                  r‹   Ú(test_linearize_pendulum_lagrange_minimalr·     s¼  € Ü	˜Ó	€BÜ
˜˜qÓ
!€CÜ�iÓ �G€A€qˆ!Ø€Aô 	�sÓ€AÜ	ˆt‹€BØ‡J�Jˆq�!Ôð 	
�‰�C˜ " a§c¡c Ó+€AØ‡M�M�!�S˜Ÿ™‘WÔð 	�‰�S˜!˜AŸC™C™%Ó €AØ‡M�M�"�a˜ÔÜ	�$˜˜1Ó	€Bô �Q˜Ó
€CÜ	˜˜r˜d°°1°Q±3°q·s±s±7¨|¨nÀAÔ	F€BØ×ÑÔ!ð —L‘L " ¨ u°d�LÓ;�M€A€qˆ'ä�A‹;œ& 1 a &¨4´°B³©<¸©>¸1Ð*=Ð!>Ó?Ò?Ð?Ð?Ø”�r“
Š?Ðˆ?ð —L‘L " ¨ u°dÈ$�LÓO�M€A€qˆ'ä�A‹;œ& 1 a &¨4´°B³©<¸©>¸1Ð*=Ð!>Ó?Ò?Ð?Ð?Ø”�r“
Š?Ð‰?r�   c                  ó‚  — t        d«      \  } }t        dd¬«      \  }}t        d«      \  }}}d}t        d«      }t        d«      }	|	j	                  |d«       t        || z  «      }
|j                  d	d
|
|j                  g«      }|	j                  d| |j                  z  ||j                  z  z   «      }|j	                  ||j                  |	«      j                  |«      «       t        d||«      }t        | dz  |dz  z   |dz  z
  g«      }t        ||«      }t!        || |g||||z  |j                  z  fg|¬«      }|j#                  «        | ||d|d|d|j%                  |«      d|j%                  |«      di}|j'                  |¬«      }|j)                  |«       |j+                  |g|g| g|g|d¬«      \  }}}t-        |«      t        ddgd|z  dgg«      k(  sJ ‚|t        g «      k(  sJ ‚|j+                  |g|g| g|g|dd„ ¬«      \  }}}t-        |«      t        ddgd|z  dgg«      k(  sJ ‚|t        g «      k(  sJ ‚y )NrŸ   r/   r    rŽ   r�   r   r�   r   r   r‘   r¢   r’   r"   )Ú
hol_coneqsr²   r³   )r-   T)r-   r.   r•   c                 ó$   — | j                  |«      S r¥   r¦   r¨   s     r‹   rª   z=test_linearize_pendulum_lagrange_nonminimal.<locals>.<lambda>L  s   € Ø!"§¡¨1£ð r�   )r-   r.   r4   )r   r   r   r   r?   r   r6   r7   r:   r8   r9   r=   r>   r   r   r   r   r´   r5   Úsolve_multipliersÚupdaterO   r   )rV   rW   r^   r_   r–   rS   rQ   rT   r   r—   r®   r   r!   r’   rF   rµ   r¶   r-   Úlam_opr   r›   s                        r‹   Ú+test_linearize_pendulum_lagrange_nonminimalr¾   )  sU  € Ü˜FÓ#�F€BˆÜ˜f¨AÔ.�H€CˆÜ�iÓ �G€A€qˆ!Ø€Aä�sÓ€AÜ	ˆt‹€BØ‡J�Jˆq�!Ôä�"�R‘%‹[€FØ	�‰�C˜ &¨!¯#©# Ó/€Aà
�‰�T˜2˜aŸc™c™6 B q§s¡s¡F™?Ó+€AØ‡I�Iˆa�—‘˜B“×"Ñ" 1Ó%Ô&Ü	�$˜˜1Ó	€Bä
�"�a‘%˜"˜a™%‘- ! Q¡$Ñ&Ð'Ó
(€Cä
�Q˜Ó
€CÜ	˜˜r 2˜h°3ÀAÀqÈÁsÈ1Ï3É3ÁwÀ<À.ÐXYÔ	Z€BØ×ÑÔ!à�A�r˜1˜c 1 c¨1¨c¯h©h°q«k¸1¸c¿h¹hÀq»kÈ1ÐM€Hà×!Ñ!¨8Ð!Ó4€FØ‡O�O�FÔà—L‘L " ¨ u¨r¨d°S°EØ tð !ó -�M€A€qˆ'ä�A‹;œ& 1 a &¨4°©6°1¨+Ð!6Ó7Ò7Ð7Ð7Ø”�r“
Š?Ðˆ?ð —L‘L " ¨ u¨r¨d°S°EÀHØ)-ñ >.ð !ó /�M€A€qˆ'ô �A‹;œ& 1 a &¨4°©6°1¨+Ð!6Ó7Ò7Ð7Ð7Ø”�r“
Š?Ð‰?r�   c                  óò  — t        d«      x\  } }}}t        dd«      x\  }}}}t        d«      \  }}	}
t        d«      }|j                  dd| |j                  g«      }|j                  dd||j
                  g«      }|j                  dd||j                  g«      }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  «      }t        d|||	||f«      }|	 |
z  |z  t        |«      z  |_        t        ||«      }t!        ||«      }|j#                  «        | d
|d
|d
|d
|d
|j%                  «       d
|j%                  «       d
|j%                  «       d
i}|j'                  |||d¬«      d
   }t)        g d¢g d¢g d¢d
d
d
d
d|z  d
gd
d|
z  d|z  z  d
d|z  dz  d
d
gg d¢g«      }||k(  sJ ‚y )Nzq1 q2 q3r/   zr m gr   ÚYr   r–   r™   r   r   ÚDmcr#   r"   ÚBodyDT)r“   Úqd_indr-   r.   )r   r   r   r/   r   r   )r   r   r   r   r/   r   )r   r   r   r   r   r/   iúÿÿÿéüÿÿÿr*   r+   )r   r   r   r   r   r   )r   r   r   r6   r7   r8   r9   r   r?   r:   r@   r   r   r   Úpotential_energyr   r   r´   r5   rO   r   )rV   rW   rX   r\   r^   r_   r`   rd   rR   rS   rT   r   rÀ   r–   r™   r   rÁ   rz   rÂ   rµ   Úlr-   r   rƒ   s                           r‹   Ú$test_linearize_rolling_disc_lagrangerÇ   Q  s  € Ü# JÓ/Ð/�J€BˆˆB�Ü'¨
°AÓ6Ð6�M€Cˆˆc�BÜ�gÓ�G€A€qˆ!ä�sÓ€AØ	�‰�C˜ " a§c¡c Ó+€AØ	�‰�C˜ " a§c¡c Ó+€AØ	�‰�C˜ " a§c¡c Ó+€Aäˆ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Ü�g˜s A q¨1¨c¨(Ó3€EØ ˜S 1™W q™[¬3¨r«7Ñ2€EÔä
�Q˜Ó
€CÜ˜˜QÓ€AØ×ÑÔ ð �A�r˜1˜b !Ø�Q˜˜QØ—‘“
˜A˜sŸx™x›z¨1¨c¯h©h«j¸!ð=€Hð 	
�‰˜! B°À4ˆÓHÈÑK€AÜ
Ò$Ú$Ú$Ø�a˜˜A˜r #™v qÐ)Ø�b˜‘d˜A˜a™C‘j ! Q s¡U¨1¡W¨a°Ð3Ú$ð&ó '€Cð �Š8€O‰8r�   N)"Úsympyr   r   r   r   r   r   r	   Úsympy.core.sympifyr
   Úsympy.simplify.simplifyr   Úsympy.solvers.solversr   Úsympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   Úsympy.testing.pytestr   rŒ   rœ   r°   r·   r¾   rÇ   © r�   r‹   ú<module>rÏ      se   ðß A× AÑ AÝ &Ý ,Ý '÷÷ ÷ ñ õ &ð ñ,ó ð,òB#òJNòb#òL&óP#r�   