§
    OŠtjÙ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!dS )é    )ÚsymbolsÚMatrixÚcosÚsinÚatanÚsqrtÚRational)Úsympify)Úsimplify)Úsolve)ÚdynamicsymbolsÚReferenceFrameÚPointÚdotÚcrossÚinertiaÚKanesMethodÚParticleÚ	RigidBodyÚ
LagrangianÚLagrangesMethod)Úslowc                  óª  ‡;‡<‡=‡>‡?‡@‡A— t          d¦  «        \  Š?} }}}t          d¦  «        x\  }}}}}}	}
ˆ?fd„|
D ¦   «         x\  }}}}}}}t          d¦  «        }t          d¦  «        x\  }}}}}}}ˆ?fd„|D ¦   «         \  }}}}}}t          d¦  «        Š=t          d¦  «        }‰=                     d	d
|‰=j        g¦  «        } |                      dd
|| j        g¦  «        }!|!                     dd
||!j        g¦  «        Š;|                     d|‰=j        z  |‰=j        z  z   |	‰=j        z  z   ¦  «        Š<‰; 	                    ‰=¦  «        ŠA|! 	                    ‰=¦  «        }"‰; 
                    ‰=||!j        z  ||!j        z  z   ||!j        z  z   ¦  «         ‰<                     |¦  «                             ‰=¦  «        Š@‰<                     ‰=|‰;j        z  |‰;j        z  z   |‰;j        z  z   ¦  «         ‰<                     d| |!j        z  ¦  «        Š>‰>                     ‰<‰=‰;¦  «         t          |	t!          ‰<                     ‰>¦  «        ‰=j        ¦  «        z
  g¦  «        }#t          ˆ=ˆ>fd„‰;D ¦   «         ¦  «        }$t          ˆ;ˆ=ˆAfd„|!D ¦   «         ˆ<ˆ=ˆ@fd„‰=D ¦   «         z   ¦  «        }%t#          |%|¦  «        }&|! 
                    ‰=|"                     |&¦  «        ¦  «         ‰;                     ‰=‰; 	                    ‰=¦  «                             |!¦  «        t)          |! 	                    ‰=¦  «        ‰; 	                    ‰=¦  «        ¦  «        z   ¦  «         ||z  | j        z  }'|| dz  z  dz  }(|| dz  z  dz  })t+          ‰;|(|)|(¦  «        }*t-          d‰<‰;||*‰<f¦  «        }+|+g},‰<|'fg}-t/          ‰=|||||g|||g|%|	g|#|||g|$¬¦  «        }.|.                     |,|-¦  «        \  }/}0|.                     ¦   «         }1|1j        |#k    sJ ‚|1j        |$k    sJ ‚|1j        |$                     ‰?¦  «                             |.                     ¦   «         ¦  «        k    sJ ‚t#          |1j        |1j         z   |¦  «        }2|& !                    ¦   «         D ]}3|2|3         |&|3         k    sJ ‚ŒtE          |1j#        |1j$        z   |/z
  |0z
  ¦  «        t          g d¢¦  «        k    sJ ‚|	|  tK          |¦  «        z  i}4|d|tM          |¦  «        |z  |z   |tK          |¦  «        |z  ||  tM          |¦  «        |z  |z   z  tK          |¦  «        z  |d||  tM          |¦  «        |z  |z   z  tM          |¦  «        z  i}5|d||  tM          |¦  «        |z  |z   z  tK          |¦  «        z  ||  tM          |¦  «        |z  |z   z  tM          |¦  «        z  |di}6|d|z  tM          |¦  «        z  d| z  z  tM          d|z  ¦  «        |dz  z  dz  z   dtK          |¦  «        z  |z  |z  dz  z   |d|d|| tM          |¦  «        tM          |¦  «        z  |z  |z  tM          |¦  «        |dz  z  z   z  || d|z  tM          |¦  «        z  d| z  z  tM          d|z  ¦  «        |dz  z  dz  z   dtK          |¦  «        z  |z  |z  dz  z   z  ||  tM          |¦  «        tK          |¦  «        z  |z  |z  tK          |¦  «        |dz  z  z   z  i}7|1 '                    |4|5|6|7gdd¬¦  «        \  } }!|d|d|d| d|di}8t          g d¢g d¢g d¢tM          |¦  «        |z  ddddtM          |¦  «         tK          |¦  «         dgtK          |¦  «         |z  ddddtK          |¦  «        tM          |¦  «         dgdtQ          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¦  «        }9t          g ¦  «        }:|                      |8¦  «        |9k    sJ ‚|!                     |8¦  «        |:k    sJ ‚tS          |                      |8¦  «                             |dtU          d"¦  «        z  ¦  «        ¦  «         +                    ¦   «         dd#ik    sJ ‚|.                     d$¬%¦  «        }1|1 '                    |4|5|6|7gdd¬¦  «        \  } }!|                      |8¦  «        |9k    sJ ‚|!                     |8¦  «        |:k    sJ ‚d S )&Nz	t r m g vzq1:7c                 ó:   •— g | ]}|                      ‰¦  «        ‘ŒS © ©Údiff)Ú.0ÚqiÚts     €új/var/www/html/CA-Chatbot/venv/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_linearize.pyú
<listcomp>z4test_linearize_rolling_disc_kane.<locals>.<listcomp>   s#   ø€ Ð(@Ð(@Ð(@¸¨¯ª°©¬Ð(@Ð(@Ð(@ó    zu:6zu1:7c                 ó:   •— g | ]}|                      ‰¦  «        ‘ŒS r   r   )r   Úuir    s     €r!   r"   z4test_linearize_rolling_disc_kane.<locals>.<listcomp>   s#   ø€ Ð#;Ð#;Ð#;°2 B§G¢G¨A¡J¤JÐ#;Ð#;Ð#;r#   ÚNÚNOÚAÚAxisÚBÚCÚCOÚPc                 óV   •— g | ]%}t          ‰                     ‰¦  «        |¦  «        ‘Œ&S r   ©r   Úvel)r   Úuvr&   r-   s     €€r!   r"   z4test_linearize_rolling_disc_kane.<locals>.<listcomp>6   s-   ø€ Ð0Ð0Ð0¨•#�a—e’e˜A‘h”h Ñ#Ô#Ð0Ð0Ð0r#   c                 ó\   •— g | ](}t          ‰‰                     ‰¦  «        z
  |¦  «        ‘Œ)S r   )r   Ú
ang_vel_in)r   r1   r+   r&   Úw_c_n_qds     €€€r!   r"   z4test_linearize_rolling_disc_kane.<locals>.<listcomp>9   s2   ø€ ÐGÐGÐG¸r•s˜8 a§l¢l°1¡o¤oÑ5°rÑ:Ô:ÐGÐGÐGr#   c                 ó\   •— g | ](}t          ‰‰                     ‰¦  «        z
  |¦  «        ‘Œ)S r   r/   )r   r1   r,   r&   Ú	v_co_n_qds     €€€r!   r"   z4test_linearize_rolling_disc_kane.<locals>.<listcomp>:   s2   ø€ ÐDÐDÐD¸B�˜Y¨¯ª°©¬Ñ2°BÑ7Ô7ÐDÐDÐDr#   é   é   Ú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   rD   )r   r   r   r   r   rD   r   r   )r   r   r   r   r   r   rD   r   )r   r   r   r   r   r   r   r   éþÿÿÿé   é   ÚGJ)Úlinear_solver),r   r   r   r   Ú	orientnewÚzÚxÚyÚ	locatenewr3   Úset_ang_velÚpos_fromÚdtÚset_velÚv2pt_theoryr   r   r   ÚsubsÚset_ang_accr   r   r   r   Úkanes_equationsÚto_linearizerÚf_cÚf_vÚf_ar   ÚkindiffdictÚf_0Úf_1Úkeysr   Úf_2Úf_3r   r   Ú	linearizer	   r
   r   Ú	eigenvals)BÚrÚmÚgÚvÚq1Úq2Úq3Úq4Úq5Úq6ÚqÚq1dÚq2dÚq3dÚq4dÚq5dÚq6dÚqdÚuÚu1Úu2Úu3Úu4Úu5Úu6Úu1dÚu2dÚu3dÚu4dÚu5dÚu6dr'   r(   r*   Úw_b_n_qdrX   rY   ÚkindiffsÚqdotsÚF_COÚIÚJÚI_C_COr9   ÚBLÚFLÚKMÚfrÚfr_starÚ
linearizerÚsolr   Úq_opÚu_opÚqd_opÚud_opÚupright_nominalÚA_solÚB_solr+   r,   r&   r-   r    r6   r4   sB                                                              @@@@@@@r!   Ú test_linearize_rolling_disc_kaner—      s|	  øøøøøøø€ õ ˜KÑ(Ô(�M€A€qˆ!ˆQ�õ "0°Ñ!7Ô!7Ð7Ñ€BˆˆB��B˜˜QØ(@Ð(@Ð(@Ð(@¸aÐ(@Ñ(@Ô(@Ð@Ñ €Cˆˆc�3˜˜S 2õ 	�uÑÔ€AÝ!/°Ñ!7Ô!7Ð7Ñ€BˆˆB��B˜˜QØ#;Ð#;Ð#;Ð#;¸Ð#;Ñ#;Ô#;Ñ €Cˆˆc�3˜˜Sõ 	�sÑÔ€AÝ	ˆt‰Œ€BØ	�Š�C˜ " a¤c Ñ+Ô+€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ	�Š�d˜B˜qœs™F R¨¬¡V™O¨b°´©fÑ4Ñ	5Ô	5€Bð �|Š|˜A‰Œ€HØ�|Š|˜A‰Œ€Hð ‡M‚M�!�R˜œ‘V˜b ¤™f‘_ r¨!¬#¡vÑ-Ñ.Ô.Ð.ð —’˜B‘”×"Ò" 1Ñ%Ô%€Ið ‡J‚Jˆq�"�Q”S‘&˜2˜aœc™6‘/ B q¤s¡FÑ*Ñ+Ô+Ð+ð 	�Š�S˜!˜AœC™%Ñ Ô €AØ‡M‚M�"�a˜ÑÔÐõ �"•s˜2Ÿ;š; q™>œ>¨1¬3Ñ/Ô/Ñ/Ð0Ñ
1Ô
1€Cõ Ð0Ð0Ð0Ð0Ð0¨aÐ0Ñ0Ô0Ñ
1Ô
1€Cõ ÐGÐGÐGÐGÐGÐGÀQÐGÑGÔGØDÐDÐDÐDÐDÐDÀ!ÐDÑDÔDñEñ Fô F€Hå�(˜BÑÔ€Eð ‡M‚M�!�X—]’] 5Ñ)Ô)Ñ*Ô*Ð*Ø‡M‚M�!�Q—\’\ !‘_”_×'Ò'¨Ñ*Ô*­U°1·<²<À±?´?ÀAÇLÂLÐQRÁOÄOÑ-TÔ-TÑTÑUÔUÐUð ˆQ‰3ˆqŒs‰7€Dð 
ˆQ�‰T‰�Q‰€AØ	
ˆQ�‰T‰�Q‰€AÝ�Q˜˜1˜aÑ Ô €Få�V˜R  A¨° |Ñ4Ô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Ô
4€CØ�jŠj‰lŒlð $ð $ˆØ�2Œw˜% œ)Ò#Ð#Ð#Ð#Ð#Ý�J”N Z¤^Ñ3°bÑ8¸7ÑBÑCÔCÅvÈiÈiÈiÑGXÔGXÒXÐXÐXÐXð ��•3�r‘7”7‘
Ð€DØ�Ø•�B‘”˜‘˜cÑ!Ø•�B‘”˜‘Ø��•C˜‘G”G˜C‘K #Ñ%Ñ&¥s¨2¡w¤wÑ.Ø�Ø��•C˜‘G”G˜C‘K #Ñ%Ñ&¥s¨2¡w¤wÑ.ð0€Dð �!Ø�1�"•c˜"‘g”g˜c‘k CÑ'Ñ(­¨R©¬Ñ0Ø�1�"•c˜"‘g”g˜c‘k CÑ'Ñ(­¨R©¬Ñ0Ø�!ð€Eð �!�A‘#•c˜"‘g”g‘+˜q ™sÑ#¥c¨!¨B©$¡i¤i°°Q±Ñ&6°qÑ&8Ñ8¸1½SÀ¹W¼W¹9ÀS¹=ÈÑ;LÈQÑ;NÑNØ�!Ø�!Ø�!•S˜‘W”W�S ™WœW‘_ SÑ(¨Ñ,­s°2©w¬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”g�c "™gœg‘o cÑ)¨#Ñ-µ°B±´¸¸Q¹±Ñ>Ñ?ðA€Eð ×Ò¨$°°e¸UÐ)CÈTÐ\`ÐÑaÔa�D€A€qà˜A˜r 1 a¨¨A¨q°!°QÐ7€Oõ Ð,Ð,Ð,Ø,Ð,Ð,Ø,Ð,Ð,Ý˜‘W”W˜S‘[ ! Q¨¨1­s°2©w¬w¨h½¸R¹¼¸À!ÐDÝ˜"‘g”g�X˜c‘\ 1 a¨¨A­s°2©w¬w½¸R¹¼¸À!ÐDØ�  A™œ¨¨1¨a°°A°q¸±u¸Q±wÐ?Ø,Ð,Ð,Ø˜˜1˜a  B s¡F¨A¨qÐ1ð3ñ 4ô 4€Eõ �2‰JŒJ€Eð �6Š6�/Ñ"Ô" eÒ+Ð+Ð+Ð+Ø�6Š6�/Ñ"Ô" eÒ+Ð+Ð+Ð+õ �1—6’6˜/Ñ*Ô*×/Ò/°°Qµt¸A±w´w±YÑ?Ô?Ñ@Ô@×JÒJÑLÔLÐQRÐTUÐPVÒVÐVÐVÐVð ×!Ò!°Ð!Ñ5Ô5€JØ×Ò¨$°°e¸UÐ)CØ(,°tð  ñ =ô =�D€A€qà�6Š6�/Ñ"Ô" eÒ+Ð+Ð+Ð+Ø�6Š6�/Ñ"Ô" eÒ+Ð+Ð+Ð+Ð+Ð+r#   c                  óX  — t          d¦  «        } t          d¦  «        }t          dd¦  «        }t          d¦  «        \  }}}d}t          d¦  «        }t          d¦  «        }|                     |d¦  «         |                     d	d
| |j        g¦  «        }	|	                     |||j        z  ¦  «         |                     d||	j	        z  ¦  «        }
|
 
                    |||	¦  «         t          d|
|¦  «        }t          ||z
  g¦  «        }||z  |j	        z  }t          || g|g|¬¦  «        }|                     |g|
|fg¦  «        \  }}|                     dd¬¦  «        \  }	}}|	t          ddgdt!          | ¦  «        z  |z  dgg¦  «        k    sJ ‚|t          g ¦  «        k    sJ ‚d S )Nrg   rv   rD   úL, m, tçš™™™™™#@r&   úN*r   r(   Úaxisr-   ÚpP)Úq_indÚu_indr:   T)rC   r   çš™™™™™#À)r   r   r   r   rR   rJ   rK   rO   rN   rL   rS   r   r   r   rV   ra   r   )rg   rv   rn   ÚLrd   r    re   r&   ÚpNr(   r-   r�   ÚkdeÚRr‹   rŒ   Úfrstarr*   Úinp_vecs                      r!   Ú$test_linearize_pendulum_kane_minimalr§   �   s¶  € Ý	˜Ñ	Ô	€BÝ	˜Ñ	Ô	€BÝ
˜˜qÑ
!Ô
!€CÝ�iÑ Ô �G€A€qˆ!Ø€Aõ 	�sÑÔ€AÝ	ˆt‰Œ€BØ‡J‚Jˆq�!ÑÔÐð 	
�Š�C˜ " a¤c Ñ+Ô+€AØ‡M‚M�!�R˜œ‘VÑÔÐð 	�Š�S˜!˜AœC™%Ñ Ô €AØ‡M‚M�"�a˜ÑÔÐÝ	�$˜˜1Ñ	Ô	€Bõ �#˜‘(�Ñ
Ô
€Cð 	
ˆ!‰ˆAŒC‰€Aõ 
�Q˜r˜d¨2¨$°sÐ	;Ñ	;Ô	;€BØ×%Ò% r d¨a°¨V¨HÑ5Ô5�L€Rˆð —L’L¨¸�LÑ=Ô=�M€A€qˆ'à•˜˜A˜ ¥c¨"¡g¤g¡¨a¡°Ð 3Ð4Ñ5Ô5Ò5Ð5Ð5Ð5Ø•�r‘
”
Š?ˆ?ˆ?ˆ?ˆ?ˆ?r#   c                  óˆ  — t          d¦  «        \  } }t          dd¬¦  «        \  }}t          d¦  «        \  }}t          dd¬¦  «        \  }}t          d¦  «        \  }}	}
d}t          d¦  «        }t          d¦  «        }|                     |d	¦  «         t          || z  ¦  «        }|                     d
d||j        g¦  «        }|                     d| |j	        z  ||j
        z  z   ¦  «        }t          d||	¦  «        }t          ||z
  ||z
  g¦  «        }t          |||g¦  «        }|                     ||                     |¦  «                             |¦  «                             |¦  «        ¦  «         t          |                     |¦  «                             ¦   «         |z
  g¦  «        }t          |                     |¦  «                             |¦  «                             |j	        ¦  «        g¦  «        }|                     ¦   «          |                     |
¦  «        }|                     ¦   «          |	|z  |j	        z  }t/          ||g|g| g|g||||¬¦	  «	        }|                     |g||fg¦  «        \  }}| ||d	i}|d	|d	i}|d	|d	i}|                     |||gdd¬¦  «        \  }}}|                     ¦   «         t          d	dgd|z  d	gg¦  «        k    sJ ‚|t          g ¦  «        k    sJ ‚|                     |||gddd¬¦  «        \  }}}|                     ¦   «         t          d	dgd|z  d	gg¦  «        k    sJ ‚|t          g ¦  «        k    sJ ‚|                     |||gddd„ ¬¦  «        \  }}}|                     ¦   «         t          d	dgd|z  d	gg¦  «        k    sJ ‚|t          g ¦  «        k    sJ ‚d S )Núq1:3rD   ©Úlevelzu1:3r™   rš   r&   r›   r   r(   rœ   ÚP1r�   )rž   rŸ   r;   r=   r<   r>   Úacceleration_constraintsr:   TrA   r    rH   )rB   rC   r   rI   c                 ó,   — |                       |¦  «        S ©N©ÚLUsolve©r(   Úbs     r!   ú<lambda>z9test_linearize_pendulum_kane_nonminimal.<locals>.<lambda>ý   s   € ¸A¿IºIÀa¹L¼L€ r#   )r   r   r   r   rR   r   rJ   rK   rN   rL   rM   r   r   r   rP   rQ   rT   Ú	magnituder0   Úexpressr   r   r   r   rV   ra   Úexpand) rg   rh   rn   ro   rv   rw   r|   r}   r¡   rd   r    re   r&   r¢   Útheta1r(   r-   r�   r£   Údq_dictrX   rY   rZ   r¤   r‹   rŒ   r¥   r�   r‘   r“   r*   r¦   s                                    r!   Ú'test_linearize_pendulum_kane_nonminimalrº   ²   sý  € õ ˜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™6 B q¤s¡F™?Ñ+Ô+€AÝ	�$˜˜1Ñ	Ô	€Bõ �#˜‘(Ø˜‘(ðñ ô €Cå�C˜#˜s˜Ñ$Ô$€Gð ‡I‚Iˆa�—’˜B‘”×"Ò" 1Ñ%Ô%×*Ò*¨7Ñ3Ô3Ñ4Ô4Ð4õ �!—*’*˜R‘.”.×*Ò*Ñ,Ô,¨qÑ0Ð1Ñ
2Ô
2€Cõ �!—%’%˜‘(”(×"Ò" 1Ñ%Ô%×)Ò)¨!¬#Ñ.Ô.Ð/Ñ
0Ô
0€CØ‡L‚L�N„N€Nð �(Š(�1‰+Œ+€CØ‡L‚L�N„N€Nð 	
ˆ!‰ˆAŒC‰€Aõ 
�Q˜r˜d¨2¨$¸R¸DØ˜¸Ø!$¸sÈ3ð
Pñ 
Pô 
P€Bð ×%Ò% r d¨a°¨V¨HÑ5Ô5�L€Rˆð ��2�qˆ>€DØ��2�qˆ>€DØ�!�S˜!Ð€Eà—L’L¨4°°uÐ*=ÀtØ*.ð !ñ 0ô 0�M€A€qˆ'ð �8Š8‰:Œ:� ! Q ¨$¨q©&°!¨Ð 5Ñ6Ô6Ò6Ð6Ð6Ð6Ø•�r‘
”
Š?ˆ?ˆ?ˆ?ð —L’L¨4°°uÐ*=ÀtØ)-¸Tð !ñ Cô C�M€A€qˆ'ð �8Š8‰:Œ:� ! Q ¨$¨q©&°!¨Ð 5Ñ6Ô6Ò6Ð6Ð6Ð6Ø•�r‘
”
Š?ˆ?ˆ?ˆ?à—L’L¨4°°uÐ*=Ø)-Ø*.Ø/HÐ/Hð !ñ Jô J�M€A€qˆ'ð
 �8Š8‰:Œ:� ! Q ¨$¨q©&°!¨Ð 5Ñ6Ô6Ò6Ð6Ð6Ð6Ø•�r‘
”
Š?ˆ?ˆ?ˆ?ˆ?ˆ?r#   c                  ó   — t          d¦  «        } t          dd¦  «        }t          d¦  «        \  }}}d}t          d¦  «        }t          d¦  «        }|                     |d¦  «         |                     dd	| |j        g¦  «        }|                     |||j        z  ¦  «         |                     d
||j	        z  ¦  «        }	|	 
                    |||¦  «         t          d|	|¦  «        }
t          ||
¦  «        }t          || g|	||z  |j	        z  fg|¬¦  «        }|                     ¦   «          |                     | g|gd¬¦  «        \  }}}t!          |¦  «        t#          ddgdt%          | ¦  «        z  |z  dgg¦  «        k    sJ ‚|t#          g ¦  «        k    s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 ‚d S )Nrg   rD   r™   rš   r&   r›   r   r(   rœ   r-   r�   )Ú	forcelistÚframeT)rC   r    rH   )rC   rI   )r   r   r   r   rR   rJ   rK   rO   rN   rL   rS   r   r   r   Úform_lagranges_equationsra   r   r   r   )rg   rn   r¡   rd   r    re   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 Ñ+Ô+€AØ‡M‚M�!�S˜œ‘WÑÔÐð 	�Š�S˜!˜AœC™%Ñ Ô €AØ‡M‚M�"�a˜ÑÔÐÝ	�$˜˜1Ñ	Ô	€Bõ �Q˜Ñ
Ô
€CÝ	˜˜r˜d°°1°Q±3°q´s±7¨|¨nÀAÐ	FÑ	FÔ	F€BØ×ÒÑ!Ô!Ð!ð —L’L " ¨ u°d�LÑ;Ô;�M€A€qˆ'å�A‰;Œ;�& 1 a &¨4µ°B±´©<¸©>¸1Ð*=Ð!>Ñ?Ô?Ò?Ð?Ð?Ð?Ø•�r‘
”
Š?ˆ?ˆ?ˆ?ð —L’L " ¨ u°dÈ$�LÑOÔO�M€A€qˆ'å�A‰;Œ;�& 1 a &¨4µ°B±´©<¸©>¸1Ð*=Ð!>Ñ?Ô?Ò?Ð?Ð?Ð?Ø•�r‘
”
Š?ˆ?ˆ?ˆ?ˆ?ˆ?r#   c                  óL  — t          d¦  «        \  } }t          dd¬¦  «        \  }}t          d¦  «        \  }}}d}t          d¦  «        }t          d¦  «        }	|	                     |d¦  «         t          || z  ¦  «        }
|                     d	d
|
|j        g¦  «        }|	                     d| |j	        z  ||j
        z  z   ¦  «        }|                     ||                     |	¦  «                             |¦  «        ¦  «         t          d||¦  «        }t          | dz  |dz  z   |dz  z
  g¦  «        }t          ||¦  «        }t!          || |g||||z  |j	        z  fg|¬¦  «        }|                     ¦   «          | ||d|d|d|                     |¦  «        d|                     |¦  «        di}|                     |¬¦  «        }|                     |¦  «         |                     |g|g| g|g|d¬¦  «        \  }}}t-          |¦  «        t          ddgd|z  dgg¦  «        k    sJ ‚|t          g ¦  «        k    s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 ‚d S )Nr©   rD   rª   r™   rš   r&   r›   r   r(   rœ   r¬   r�   r7   )Ú
hol_coneqsr¼   r½   )rB   T)rB   rC   r    c                 ó,   — |                       |¦  «        S r¯   r°   r²   s     r!   r´   z=test_linearize_pendulum_lagrange_nonminimal.<locals>.<lambda>L  s   € Ø!"§¢¨1¡¤ð r#   )rB   rC   rI   )r   r   r   r   rR   r   rJ   rK   rN   rL   rM   rP   rQ   r   r   r   r   r¾   r   Úsolve_multipliersÚupdatera   r   )rg   rh   rn   ro   r¡   rd   r    re   r&   r¢   r¸   r(   r-   r�   rX   r¿   rÀ   rB   Úlam_opr*   r¦   s                        r!   Ú+test_linearize_pendulum_lagrange_nonminimalrÈ   )  sÂ  € Ý˜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™6 B q¤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ÁwÀ<À.ÐXYÐ	ZÑ	ZÔ	Z€BØ×ÒÑ!Ô!Ð!à�A�r˜1˜c 1 c¨1¨c¯hªh°q©k¬k¸1¸c¿hºhÀq¹k¼kÈ1ÐM€Hà×!Ò!¨8Ð!Ñ4Ô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Ð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Ð7Ð7Ø•�r‘
”
Š?ˆ?ˆ?ˆ?ˆ?ˆ?r#   c                  ó„  — t          d¦  «        x\  } }}}t          dd¦  «        x\  }}}}t          d¦  «        \  }}	}
t          d¦  «        }|                     dd| |j        g¦  «        }|                     dd||j        g¦  «        }|                     dd||j        g¦  «        }t          d	¦  «        }|                     |d
¦  «         | 	                    d||j        z  ¦  «        }| 
                    |||¦  «         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!          ||¦  «        }|                     ¦   «          | d
|d
|d
|d
|d
|                     ¦   «         d
|                     ¦   «         d
|                     ¦   «         d
i}|                     |||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 ‚d S )Nzq1 q2 q3rD   zr m gr&   ÚYr)   r¡   r¤   r+   r   ÚDmcr8   r7   ÚBodyDT)rž   Úqd_indrB   rC   )r   r   r   rD   r   r   )r   r   r   r   rD   r   )r   r   r   r   r   rD   iúÿÿÿéüÿÿÿr?   r@   )r   r   r   r   r   r   )r   r   r   rJ   rK   rL   rM   r   rR   rN   rS   r   r   r   Úpotential_energyr   r   r¾   r   ra   r   )rg   rh   ri   rm   rn   ro   rp   rt   rc   rd   re   r&   rÊ   r¡   r¤   r+   rË   r†   rÌ   r¿   ÚlrB   r(   r�   s                           r!   Ú$test_linearize_rolling_disc_lagrangerÑ   Q  st  € Ý# JÑ/Ô/Ð/�J€BˆˆB�Ý'¨
°AÑ6Ô6Ð6�M€Cˆˆc�BÝ�gÑÔ�G€A€qˆ!å�sÑÔ€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ	�Š�C˜ " a¤c Ñ+Ô+€Aåˆ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Ý�g˜s A q¨1¨c¨(Ñ3Ô3€EØ ˜S 1™W q™[­3¨r©7¬7Ñ2€EÔå
�Q˜Ñ
Ô
€CÝ˜˜QÑÔ€AØ×ÒÑ Ô Ð ð �A�r˜1˜b !Ø�Q˜˜QØ—’‘
”
˜A˜sŸxšx™zœz¨1¨c¯hªh©j¬j¸!ð=€Hð 	
�Š˜! B°À4ˆÑHÔHÈÔK€AÝ
Ð$Ð$Ð$Ø$Ð$Ð$Ø$Ð$Ð$Ø�a˜˜A˜r #™v qÐ)Ø�b˜‘d˜A˜a™C‘j ! Q s¡U¨1¡W¨a°Ð3Ø$Ð$Ð$ð&ñ 'ô '€Cð �Š8ˆ8ˆ8ˆ8ˆ8ˆ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#   r!   ú<module>rØ      s‚  ðØ AÐ AÐ AÐ AÐ AÐ AÐ AÐ AÐ AÐ AÐ AÐ AÐ AÐ AÐ AÐ AÐ AÐ AØ &Ð &Ð &Ð &Ð &Ð &Ø ,Ð ,Ð ,Ð ,Ð ,Ð ,Ø 'Ð 'Ð 'Ð 'Ð 'Ð 'ðð ð ð ð ð ð ð ð ð ð ð ð ð ð ð ð ð ð ð ð ð ð ð ð ð ð &Ð %Ð %Ð %Ð %Ð %ð ð,ð ,ñ „ð,ðB#ð #ð #ðJNð Nð Nðb#ð #ð #ðL&ð &ð &ðP#ð #ð #ð #ð #r#   