§
    OŠtj8K  ã                   ó–   — 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 d„ Zd„ Zd„ Zd	„ Zd
S )é    )ÚcosÚMatrixÚsinÚzerosÚtanÚpiÚsymbols)Úsimplify)Útrigsimp)Úsolve)
ÚcrossÚdotÚdynamicsymbolsÚfind_dynamicsymbolsÚKanesMethodÚinertiaÚinertia_of_point_massÚPointÚReferenceFrameÚ	RigidBodyc                  ó  ‡:‡;‡<‡=‡>‡?‡@‡A‡B‡C‡D— t          d¦  «        \  ŠC} }}}}t          d¦  «        \  }}}t          d¦  «        }ˆCfd„|D ¦   «         }	t          d¦  «        }
ˆCfd„|
D ¦   «         }t          t          |dgt	          |¦  «        z  ¦  «        ¦  «        }t          d¦  «        }t          t          |dgt	          |¦  «        z  ¦  «        ¦  «        }t          d	¦  «        Š=‰=                     d
d|d         ‰=j        g¦  «        }|                     dd|d         |j        g¦  «        }|                     dd|d         |j	        g¦  «        Š:‰: 
                    ‰=|
d         |j        z  |
d         |j	        z  z   |
d         |j        z  z   ¦  «         ‰:                     ‰=‰:                     ‰=¦  «                             ‰C|¦  «        t          |                     ‰=¦  «        ‰:                     ‰=¦  «        ¦  «        z   ¦  «         t          d¦  «        Š?‰?                     ‰=|d         |j        z  |d         |j	        z  z   |d         |j        z  z   ¦  «         ‰?                     d|d         |j        z  | t%          |d         ¦  «        z  |j	        z  z   ¦  «        Š>‰>                     ‰=|
d         |j        z  |
d         |j	        z  z   |
d         |j        z  z   ¦  «         ‰>                     ‰=‰>                     ‰=¦  «                             ‰C|¦  «        t          |                     ‰=¦  «        ‰>                     ‰=¦  «        ¦  «        z   ¦  «         |	d         |j        z  |	d         |j        z  z   |	d         |j	        z  z   ŠD‰>                     ‰?¦  «                             ‰C|¦  «        t          |                     ‰=¦  «        ‰>                     ‰?¦  «        ¦  «        z   }t-          ˆ:ˆ=ˆDfd„|D ¦   «         t/          |‰>                     ‰=¦  «        z
  |j        ¦  «        gz   ¦  «        }t1          ||	¦  «        }t1          |                     |	d         d|	d         di¦  «        |
¦  «        }|                     |	d         d|	d         di¦  «         ˆ:ˆ=fd„|
|z   D ¦   «         }ˆ=ˆ>fd„|
|z   D ¦   «         }ˆ=ˆ?fd„|
|z   D ¦   «         }t-          t/          |  |j        z  |j        ¦  «        |d         z
  g¦  «        }t-          ˆ:ˆ=ˆ>ˆ?fd„|D ¦   «         ¦  «        }t          ‰:                     ‰=¦  «        ‰>                     ‰?¦  «        ¦  «        }|                     ‰C|¦  «        t          |                     ‰=¦  «        |¦  «        z   ŠBt-          ˆ=ˆ>ˆBfd„|D ¦   «         ¦  «        }t7          dd¦  «        }t9          d¦  «        D ]:}t;          |
|z   ¦  «        D ]%\  }}||                              |¦  «        |||f<   Œ&Œ;|d d …d d…f         } |d d …dd…f         }!|d d …dd …f         }"|                      |"¦  «        }#|!                     ¦   «          |#z  }$|$d d …d d…f         t-          |
d d…         ¦  «        z  }%t          t          |
dd …         |%¦  «        ¦  «        }&||z  |j        z  Š;||j        z  ||j	        z  z   ||j        z  z   Š<t-          ˆ;ˆ<fd„t          ||¦  «        D ¦   «         ¦  «        }'| ‰>                      ‰=¦  «        z  Š@tC          ||||¦  «        }(t/          |(‰: "                    ‰=¦  «        ¦  «        t          ‰:                     ‰=¦  «        t/          |(‰:                     ‰=¦  «        ¦  «        ¦  «        z    ŠAt-          ˆ@ˆAfd„t          ||¦  «        D ¦   «         ¦  «        })|'d d…d d …f          #                    |'dd …d d …f         ¦  «        |$j$        |'dd…d d …f         z  z   }*|)d d…d d …f          #                    |)dd …d d …f         ¦  «        |$j$        |)dd…d d …f         z  z   }+|+                     |¦  «                             |&¦  «                             |¦  «                             |d         |  tK          |d         ¦  «        z  i¦  «         &                    ¦   «         },|(‰>f}-tO          d ‰>‰:||-¦  «        }.|.g}/‰>‰;f}0‰?‰<f}1|0|1g}2tQ          ‰=|d d…         |
d d…         ||dd …         ||
dd …         ||¬!¦	  «	        }3|3 )                    |/|2¦  «        \  }4}5|5                     |¦  «                             |&¦  «                             |¦  «                             |d         |  tK          |d         ¦  «        z  i¦  «         &                    ¦   «         }6|3 *                    ¦   «         }7t-          |*¦  «         &                    ¦   «         |4 &                    ¦   «         k    sJ ‚t-          |+                     |7¦  «        ¦  «         &                    ¦   «         |5 &                    ¦   «         k    sJ ‚tW          t-          |,¦  «         &                    ¦   «         ¦  «         ,                    ddi¦  «        tW          |6 &                    ¦   «         ¦  «         ,                    ddi¦  «        k    sJ ‚t[          |3j.        ¦  «        }8|	D ]}9|9|8vsJ ‚Œ	d S )"Nzt r m g I JzFx Fy Fzzq:4c                 ó:   •— g | ]}|                      ‰¦  «        ‘ŒS © ©Údiff)Ú.0ÚqiÚts     €úf/var/www/html/CA-Chatbot/venv/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_kane2.pyú
<listcomp>z test_aux_dep.<locals>.<listcomp>1   ó#   ø€ Ð	!Ð	!Ð	!˜ˆ"�'Š'�!‰*Œ*Ð	!Ð	!Ð	!ó    zu:6c                 ó:   •— g | ]}|                      ‰¦  «        ‘ŒS r   r   )r   Úuir   s     €r   r    z test_aux_dep.<locals>.<listcomp>3   r!   r"   g        zua:3ÚNÚAÚAxisr   ÚBé   ÚCé   ÚPÚOé   é   é   c                 ó\   •— g | ](}t          ‰‰                     ‰¦  «        z
  |¦  «        ‘Œ)S r   )r   Ú
ang_vel_in)r   Úuvr*   r%   Úw_c_n_qds     €€€r   r    z test_aux_dep.<locals>.<listcomp>X   s2   ø€ ÐGÐGÐG¸r•s˜8 a§l¢l°1¡o¤oÑ5°rÑ:Ô:ÐGÐGÐGr"   c                 ób   •— g | ]+}‰                      ‰¦  «                             |‰¦  «        ‘Œ,S r   )r2   r   )r   r$   r*   r%   s     €€r   r    z test_aux_dep.<locals>.<listcomp>b   s3   ø€ Ð@Ð@Ð@°2�1—<’< ‘?”?×'Ò'¨¨AÑ.Ô.Ð@Ð@Ð@r"   c                 ób   •— g | ]+}‰                      ‰¦  «                             |‰¦  «        ‘Œ,S r   ©Úvelr   )r   r$   r%   r-   s     €€r   r    z test_aux_dep.<locals>.<listcomp>c   ó1   ø€ Ð9Ð9Ð9¨B�1—5’5˜‘8”8—=’=  QÑ'Ô'Ð9Ð9Ð9r"   c                 ób   •— g | ]+}‰                      ‰¦  «                             |‰¦  «        ‘Œ,S r   r7   )r   r$   r%   r,   s     €€r   r    z test_aux_dep.<locals>.<listcomp>d   r9   r"   c                 ó  •— g | ]„}t          ‰                     ‰¦  «        ‰                     ‰¦  «        t          ‰                     ‰¦  «        ‰                     ‰¦  «        ¦  «        z   z
  |¦  «                             ¦   «         ‘Œ…S r   )r   r8   r   r2   Úpos_fromÚexpand)r   Úair*   r%   r-   r,   s     €€€€r   r    z test_aux_dep.<locals>.<listcomp>k   s†   ø€ ð 3ð 3ð 3Ø*,õ �a—e’e˜A‘h”h !§%¢%¨¡(¤(­U°1·<²<À±?´?Ø	�
Š
�1‰Œñ.ô .ñ #ñ Øñô ß#šV™XœXð3ð 3ð 3r"   c                 ó\   •— g | ](}t          ‰                     ‰¦  «        ‰z
  |¦  «        ‘Œ)S r   )r   Úacc)r   r>   r%   r-   Úa_o_ns     €€€r   r    z test_aux_dep.<locals>.<listcomp>o   s2   ø€ Ð8Ð8Ð8°•#�a—e’e˜A‘h”h Ñ&¨Ñ+Ô+Ð8Ð8Ð8r"   é	   é   c                 óX   •— g | ]&\  }}t          ‰|¦  «        t          ‰|¦  «        z   ‘Œ'S r   ©r   )r   Úpv_oÚpv_pÚF_OÚF_Ps      €€r   r    z test_aux_dep.<locals>.<listcomp>Š   s>   ø€ ð +ð +ð +±z°t¸T•3�s˜D‘>”>¥C¨¨T¡N¤NÑ2ð +ð +ð +r"   c                 óX   •— g | ]&\  }}t          ‰|¦  «        t          ‰|¦  «        z   ‘Œ'S r   rE   )r   ÚpvÚpavÚR_star_OÚT_star_Cs      €€r   r    z test_aux_dep.<locals>.<listcomp>•   sB   ø€ ð 7ð 7ð 7Á7À2Às�˜H bÑ)Ô)­C°¸#Ñ,>Ô,>Ñ>ð 7ð 7ð 7r"   Údisc)Úq_indÚu_indÚkd_eqsÚq_dependentÚconfiguration_constraintsÚu_dependentÚvelocity_constraintsÚu_auxiliary)/r	   r   ÚdictÚzipÚlenr   Ú	orientnewÚzÚxÚyÚset_ang_velÚset_ang_accr2   r   r   r   Úset_velÚ	locatenewr   Úset_accr8   r<   r   r   r   ÚsubsÚupdater   ÚrangeÚ	enumerateÚrow_joinÚinvr@   r   Ú
ang_acc_inÚcol_joinÚTr   r=   r   r   Úkanes_equationsÚkindiffdictr
   Úxreplacer   Úforcing)EÚrÚmÚgÚIÚJÚFxÚFyÚFzÚqÚqdÚuÚudÚud_zeroÚuaÚua_zeror&   r(   Úv_o_n_qdÚkindiffsÚqd_kdÚsteady_conditionsÚpartial_w_CÚpartial_v_OÚpartial_v_PÚf_cÚf_vÚv_o_nÚf_aÚM_vÚiÚjr$   ÚM_v_iÚM_v_dÚM_v_auxÚ	M_v_i_auxÚA_rsÚu_depÚ
u_dep_dictÚFr_uÚI_C_OÚ	Fr_star_uÚFr_cÚ	Fr_star_cÚFr_star_steadyÚ
iner_tuplerO   ÚbodyListÚF_oÚF_pÚ	forceListÚkaneÚfrÚfrstarÚfrstar_steadyÚkddÚsyms_in_forcingÚqdir*   rH   rI   r%   r-   r,   rM   rN   rA   r   r4   sE                                                             @@@@@@@@@@@r   Útest_aux_depr§      sÔ
  øøøøøøøøøøø€ õ" ˜}Ñ-Ô-Ñ€A€qˆ!ˆQ��1Ý˜Ñ$Ô$�J€BˆˆBõ& 	�uÑÔ€AØ	!Ð	!Ð	!Ð	!˜qÐ	!Ñ	!Ô	!€BÝ�uÑÔ€AØ	!Ð	!Ð	!Ð	!˜qÐ	!Ñ	!Ô	!€BÝ•3�r˜B˜4¥ B¡¤™<Ñ(Ô(Ñ)Ô)€GÝ	˜Ñ	Ô	€BÝ•3�r˜B˜4¥ B¡¤™<Ñ(Ô(Ñ)Ô)€Gõ 	�sÑÔ€AØ	�Š�C˜ ! A¤$¨¬ Ñ-Ô-€AØ	�Š�C˜ ! A¤$¨¬ Ñ-Ô-€AØ	�Š�C˜ ! A¤$¨¬ Ñ-Ô-€Að ‡M‚M�!�Q�q”T˜!œ#‘X  !¤ Q¤S¡Ñ(¨1¨Q¬4°´©8Ñ3Ñ4Ô4Ð4Ø‡M‚M�!�Q—\’\ !‘_”_×)Ò)¨!¨QÑ/Ô/Ý˜1Ÿ<š<¨™?œ?¨A¯LªL¸©O¬OÑ<Ô<ñ=ñ >ô >ð >õ 	ˆc‰
Œ
€AØ‡I‚Iˆa��A”�q”s‘˜R œU 1¤3™YÑ&¨¨A¬¨q¬s©Ñ2Ñ3Ô3Ð3Ø	�Š�C˜˜1œ˜aœc™ A¥c¨!¨A¬$¡i¤i¡K°´¡OÑ3Ñ4Ô4€AØ‡I‚Iˆa��1”�a”c‘˜A˜aœD ¤™HÑ$ q¨¤t¨A¬C¡xÑ/Ñ0Ô0Ð0Ø‡I‚Iˆa�—’�q‘”—’˜q !Ñ$Ô$¥u¨Q¯\ª\¸!©_¬_¸a¿eºeÀA¹h¼hÑ'GÔ'GÑGÑHÔHÐHð �!Œu�Q”S‰y˜2˜aœ5 ¤™9Ñ$ r¨!¤u¨Q¬S¡yÑ0€HØ�zŠz˜!‰}Œ}×!Ò! ! QÑ'Ô'­%°·²¸Q±´ÀÇÂÈAÁÄÑ*OÔ*OÑO€HÝÐGÐGÐGÐGÐGÐGÀQÐGÑGÔGÝ˜8 a§e¢e¨A¡h¤hÑ.°´Ñ4Ô4Ð5ñ6ñ 7ô 7€Hå�(˜BÑÔ€Eõ ˜hŸmšm¨R°¬U°Q¸¸1¼ÀÐ,BÑCÔCÀQÑGÔGÐØ×Ò˜b œe a¨¨A¬°Ð3Ñ4Ô4Ð4ð AÐ@Ð@Ð@Ð@¸¸R¹Ð@Ñ@Ô@€KØ9Ð9Ð9Ð9Ð9°!°b±&Ð9Ñ9Ô9€KØ9Ð9Ð9Ð9Ð9°!°b±&Ð9Ñ9Ô9€Kõ •#�q�b˜œ‘f˜aœcÑ"Ô" Q q¤TÑ)Ð*Ñ
+Ô
+€CÝ
ð 3ð 3ð 3ð 3ð 3ð 3ð 3Ø01ð3ñ 3ô 3ñ 4ô 4€Cå�!—,’,˜q‘/”/ 1§:¢:¨a¡=¤=Ñ1Ô1€EØ�JŠJ�q˜!ÑÔ�u Q§\¢\°!¡_¤_°eÑ<Ô<Ñ<€EÝ
Ð8Ð8Ð8Ð8Ð8Ð8°aÐ8Ñ8Ô8Ñ
9Ô
9€Cõ ��1‰+Œ+€CÝ�1‰XŒXð (ð (ˆÝ˜q 2™vÑ&Ô&ð 	(ð 	(‰EˆAˆrØ˜AœŸš B™œˆC��1�‰IˆIð	(ð ����2�A�2�ŒJ€EØ����1�Q�3�ŒK€EØ�!�!�!�Q�R�R�%Œj€GØ—’˜wÑ'Ô'€IØ�YŠY‰[Œ[ˆ=˜9Ñ$€Dà����B�Q�B�ŒK�&  2 A 2¤™-œ-Ñ'€EÝ•c˜!˜A˜B˜Bœ% Ñ'Ô'Ñ(Ô(€Jð ˆA‰#ˆaŒc‰'€CØ
ˆqŒs‰(�R˜!œ#‘XÑ
  Q¤S¡Ñ
(€CÝð +ð +ð +ð +ð +Ý�˜[Ñ)Ô)ð+ñ +ô +ñ ,ô ,€Dð ˆr�!—%’%˜‘(”(‰{€HÝ�A�q˜!˜QÑÔ€EÝ�U˜AŸLšL¨™OœOÑ,Ô,Ý˜Ÿš a™œ­#¨e°Q·\²\À!±_´_Ñ*EÔ*EÑFÔFñGð H€Håð 7ð 7ð 7ð 7ð 7Ý˜K¨Ñ5Ô5ð7ñ 7ô 7ñ 8ô 8€Ið
 ����A�A�A�Œ;×Ò  Q R R¨¨¨ U¤Ñ,Ô,¨t¬v¸¸Q¸q¸SÀ!À!À!¸V¼Ñ/DÑD€DØ˜"˜1˜"˜a˜a˜a˜%Ô ×)Ò)¨)°A°B°B¸¸¸°EÔ*:Ñ;Ô;Ø”&˜9 Q q S¨!¨!¨! VÔ,Ñ,ñ-€Ià—^’^ GÑ,Ô,×1Ò1°*Ñ=Ô=ßŠTÐ#Ñ$Ô$§T¢T¨1¨Q¬4°!°µC¸¸!¼±I´I±Ð*>Ñ%?Ô%?ÇÂÁÄð ð ˜�€JÝ�V˜Q  1 jÑ1Ô1€DØˆv€Hð ˆcˆ(€CØˆcˆ(€CØ�s�€Iõ Ø	�!�B�Q�B”%  " 1 "¤¨hØ�a�b�b”E°sØ�a�b�b”E°Øð	
ñ 
ô 
€Dð ×&Ò& x°Ñ;Ô;�L€RˆØ—K’K Ñ(Ô(×-Ò-¨jÑ9Ô9×>Ò>Ð?PÑQÔQß’T˜1˜Qœ4 ! ¥C¨¨!¬¡I¤I¡Ð.Ñ/Ô/·²±´ð à
×
Ò
Ñ
Ô
€Cå�$‰<Œ<×ÒÑ Ô  B§I¢I¡K¤KÒ/Ð/Ð/Ð/Ý�)—.’. Ñ%Ô%Ñ&Ô&×-Ò-Ñ/Ô/°6·=²=±?´?ÒBÐBÐBÐBõ •V˜NÑ+Ô+×2Ò2Ñ4Ô4Ñ5Ô5×>Ò>ÀÀ#¸wÑGÔGÝ�]×)Ò)Ñ+Ô+Ñ,Ô,×5Ò5°q¸°gÑ>Ô>ò?ð ?ð ?ð ?õ *¨$¬,Ñ7Ô7€OØð *ð *ˆØ˜/Ð)Ð)Ð)Ð)Ð)ð*ð *r"   c                  ó¼  ‡A‡B— t          d¦  «        \  } }}t          dd¬¦  «        \  }}}t          d¦  «        \  }}}}	}
t          d¦  «        \  }}}}}}}t          d¦  «        \  }}}}}}}}t          d¦  «        \  }}}t          d¦  «        \  }}}t          d	¦  «        ŠB‰B                     d
d| ‰Bj        g¦  «        } |                      dd| | j        g¦  «        ŠA‰A                     ‰B|‰Aj        z  |‰Aj        z  z   ¦  «         ‰A                     dd|‰Aj        g¦  «        }!‰A                     dd|‰Aj        g¦  «        }"|!                     ‰A|	‰Aj        z  ¦  «         |"                     ‰A|
‰Aj        z  ¦  «         t          d¦  «        }#|# 	                    ‰Ad¦  «         |# 	                    ‰B|‰Aj        z  ¦  «         |# 
                    d|‰Aj        z  ¦  «        }$|# 
                    d|‰Aj        z  |‰Aj        z  z
  ¦  «        }%|$|%fD ]}&|&                     |#‰B‰A¦  «         Œ|# 
                    d|‰Aj        z  ¦  «        }'|# 
                    d|‰Aj        z  ¦  «        }(|# 
                    d| ‰Aj        z  ¦  «        })|'|(|)fD ]}&|&                     |#‰B‰A¦  «         Œ|( 
                    d| ‰Aj        z  ¦  «        }*|) 
                    d| ‰Aj        z  ¦  «        }+|*                     |(‰B|!¦  «         |+                     |)‰B|"¦  «         ||z
  ||	z
  ||
z
  g},ˆAˆBfd„|*|+fD ¦   «         }-t          ‰A|||d|d¦  «        }.t          |!|||¦  «        }/t          |"|||¦  «        }0t          d|'‰A||.|'f¦  «        }1t          d|(|!||/|(f¦  «        }2t          d|)|"||0|)f¦  «        }3t          ‰B| ||g||g|,|	|
g|-|g¬¦  «        }4|$| |z  ‰Bj        z  f|%|‰Aj        z  |‰Aj        z  z   |‰Aj        z  z   fg}5|1|2|3g}6|4                     |6|5¦  «        \  }7}8t!          |-|	|
g¦  «        }9t#          |d|z  |dz  z  |dz  z  z   d|z  z   ||dz  z  z   d|z  |dz  z  z    |                     |¦  «        z  ||z  |z  |z  z
  |d|z  z   d|z  |dz  z  z    |                     |¦  «        z  ||z  |dz  z  z   dg¦  «        }:t'          |8                     |9¦  «                             |di¦  «        ¦  «                             ¦   «                              ¦   «         }|:|z
                       ¦   «         t/          dd¦  «        k    sJ ‚g };t1          |1|2|3g|.|/|0g¦  «        D ]r\  }<}=|=t3          |<j        |<j                             |#¦  «        |<j        ¦  «        z   }>|;                     t          d|<j        |<j        |<j        |>|#f¦  «        ¦  «         Œs|4                     |;|5¦  «        \  }?}@t'          |@                     |9¦  «                             |di¦  «        ¦  «                             ¦   «         }|:|z
                       ¦   «         t/          dd¦  «        k    sJ ‚d S ) Núq1:4r)   ©Úlevelzu1:6úu' R, M, g, e, f, thetaúa b mA mB IA J K tz	Q1, Q2 Q3úIA22 IA23 IA33ÚFr,   Úaxisr&   r(   r*   ÚDr   úS*ÚQúA*úB*úC*úB^úC^c                 ó`   •— g | ]*}t          |                     ‰¦  «        ‰j        ¦  «        ‘Œ+S r   ©r   r8   r^   ©r   Úpr&   r¯   s     €€r   r    z,test_non_central_inertia.<locals>.<listcomp>ú   s/   ø€ Ð	7Ð	7Ð	7 �#ˆa�eŠe�A‰hŒh˜œÑ
Ô
Ð	7Ð	7Ð	7r"   ÚrbAÚrbBÚrbC)rP   rQ   rR   rU   rV   rW   r+   r.   Ú )r   r	   r   r[   r^   r]   r_   r\   r   ra   rb   Úv2pt_theoryr   r   r   rm   r   r   r   r   rd   Údoitr=   r   rY   r   ÚmassÚ
masscenterr<   ÚframeÚappend)CÚq1Úq2Úq3Úq1dÚq2dÚq3dÚu1Úu2Úu3Úu4Úu5Úu_primeÚRÚMrs   ÚeÚfÚthetaÚaÚbÚmAÚmBÚIAru   ÚKr   ÚQ1ÚQ2ÚQ3ÚIA22ÚIA23ÚIA33r,   r(   r*   ÚpDÚpS_starÚpQr¼   ÚpA_starÚpB_starÚpC_starÚpB_hatÚpC_hatÚkdeÚvcÚ	inertia_AÚ	inertia_BÚ	inertia_Cr½   r¾   r¿   ÚkmÚforcesÚbodiesr¡   Úfr_starÚvc_mapÚfr_star_expectedÚbodies2ÚrbÚI_starrt   Úfr2Úfr_star2r&   r¯   sC                                                                    @@r   Útest_non_central_inertiarü   Ç   sá  øø€ õ   Ñ'Ô'�J€BˆˆBÝ" 6°Ð3Ñ3Ô3�M€CˆˆcÝ'¨Ñ/Ô/Ñ€BˆˆB��BÝ$+Ð,FÑ$GÔ$GÑ!€GˆQ��1�a˜˜EÝ 'Ð(<Ñ =Ô =Ñ€A€qˆ"ˆb�"�a˜˜AÝ˜Ñ%Ô%�J€BˆˆBÝÐ/Ñ0Ô0Ñ€Dˆ$�õ 	�sÑÔ€AØ	�Š�C˜ 5 &¨!¬# Ñ/Ô/€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ‡M‚M�!�R˜œ‘V˜b ¤™f‘_Ñ%Ô%Ð%à	�Š�C˜ " a¤c Ñ+Ô+€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ‡M‚M�!�R˜!œ#‘XÑÔÐØ‡M‚M�!�R˜!œ#‘XÑÔÐõ 
ˆs‰Œ€BØ‡J‚Jˆq�!ÑÔÐà‡J‚Jˆq�"�q”s‘(ÑÔÐà�lŠl˜4  1¤3¡Ñ'Ô'€GØ	�Š�c˜1˜QœS™5 1 Q¤S¡5™=Ñ	)Ô	)€BØ�rˆ]ð  ð  ˆØ	�Š�b˜!˜QÑÔÐÐð �lŠl˜4  1¤3¡Ñ'Ô'€GØ�lŠl˜4  1¤3¡Ñ'Ô'€GØ�lŠl˜4 !  A¤C¡Ñ(Ô(€GØ�w Ð(ð  ð  ˆØ	�Š�b˜!˜QÑÔÐÐð ×Ò˜t a R¨¬¡VÑ,Ô,€FØ×Ò˜t a R¨¬¡VÑ,Ô,€FØ
×Ò�w  1Ñ%Ô%Ð%Ø
×Ò�w  1Ñ%Ô%Ð%ð �‰8�S˜2‘X˜s R™xÐ
(€CØ	7Ð	7Ð	7Ð	7Ð	7 v¨vÐ&6Ð	7Ñ	7Ô	7€Bõ ˜˜2˜t T¨1¨d°AÑ6Ô6€IÝ˜˜1˜a Ñ#Ô#€IÝ˜˜1˜a Ñ#Ô#€Iõ �E˜7 A r¨I°wÐ+?Ñ
@Ô
@€CÝ
�E˜7 A r¨I°wÐ+?Ñ
@Ô
@€CÝ
�E˜7 A r¨I°wÐ+?Ñ
@Ô
@€Cå	�Q˜r 2 r˜l°2°r°(À3Ø"$ b ÀØ"$ ð
'ñ 
'ô 
'€Bð ˜˜˜1™˜QœS™Ð! B¨¨1¬3©°°A´C±©¸"¸Q¼S¹&Ñ(@Ð#AÐB€FØ�3˜ˆ_€FØ×$Ò$ V¨VÑ4Ô4�K€BˆÝ�2˜˜B�xÑ Ô €Fõ Ø�1�Q‘3�q˜!‘t‘8˜A˜q™D‘=Ñ  1 Q¡3Ñ&Ø��A‘‰gñØ˜"™˜Q ™T™	ñ"ð #Ø%'§W¢W¨Q¡Z¤Zñ0Ø24°Q±$°r±'¸"±*ñ=à�1�R‘4‰i˜˜1™˜Q ™T™Ñ!Ð" R§W¢W¨Q¡Z¤ZÑ/°"°Q±$°r¸1±u±*Ñ<Øð	ñ ô Ðõ
 	�—’˜fÑ%Ô%×*Ò*¨B°¨7Ñ3Ô3Ñ4Ô4×9Ò9Ñ;Ô;×BÒBÑDÔD€AØ Ñ!×)Ò)Ñ+Ô+­u°Q¸©{¬{Ò:Ð:Ð:Ð:ð €GÝ˜3  S˜/¨I°yÀ)Ð+LÑMÔMð +ð +‰
ˆˆFØÕ*¨2¬7Ø+-¬=×+AÒ+AÀ"Ñ+EÔ+EØ+-¬8ñ5ô 5ñ 5ˆð 	�Š•y  R¤]°B´H¸b¼gØ"# R ñ*ô *ñ 	+ô 	+ð 	+ð 	+à×&Ò& w°Ñ7Ô7�M€Cˆå�—’˜vÑ&Ô&×+Ò+¨R°¨GÑ4Ô4Ñ5Ô5×:Ò:Ñ<Ô<€AØ˜qÑ ×(Ò(Ñ*Ô*­e°A°q©k¬kÒ9Ð9Ð9Ð9Ð9Ð9r"   c            	      ó4  ‡;‡<— t          d¦  «        \  } }}t          dd¬¦  «        \  }}}t          d¦  «        \  }}}t          d¦  «        \  }	}
}}}}}t          d¦  «        \  }}}}}}}}t          d¦  «        \  }}}t          d¦  «        \  }}}t          d	¦  «        Š<‰<                     d
d| ‰<j        g¦  «        }|                     dd| |j        g¦  «        Š;‰;                     ‰<|‰;j        z  |‰;j        z  z   ¦  «         ‰;                     dd|‰;j        g¦  «        }‰;                     dd|‰;j        g¦  «        } t          d¦  «        }!|! 	                    ‰;d¦  «         |! 	                    ‰<|‰;j        z  ¦  «         |! 
                    d|‰;j        z  ¦  «        }"|! 
                    d|‰;j        z  |
‰;j        z  z
  ¦  «        }#|! 
                    d|‰;j        z  ¦  «        }$|! 
                    d|‰;j        z  ¦  «        }%|! 
                    d| ‰;j        z  ¦  «        }&|"|#|$|%|&fD ]}'|'                     |!‰<‰;¦  «         Œ|% 
                    d|
 ‰;j        z  ¦  «        }(|& 
                    d|
 ‰;j        z  ¦  «        })|(                     |%‰<|¦  «         |)                     |&‰<| ¦  «         ˆ;ˆ<fd„|(|)fD ¦   «         }*|*||z
  gz  }*t          |*|||g¦  «        }+t          |+                     ¦   «         ¦  «        D ]0\  },}-|-                     |¦  «        |+|,                     |¦  «        <   Œ1t!          ‰;|||d|d¦  «        }.t!          ||||¦  «        }/t!          | |||¦  «        }0t#          d|$‰;||.|$f¦  «        }1t#          d|%|||/|%f¦  «        }2t#          d|&| ||0|&f¦  «        }3t%          ‰<| ||g||g|*|g¬¦  «        }4|"| |z  ‰<j        z  f|#|‰;j        z  |‰;j        z  z   |‰;j        z  z   fg}5|1|2|3g}6t'          ||z  ||z  |z  t)          |¦  «        z  t+          | ¦  «        z  z   |||z  t)          |¦  «        z  t)          | ¦  «        z  z   ||z  |z  t+          |¦  «        z  ||z  z
  ||
z  z
  g¦  «        }7t'          |d|z  |dz  z  |
dz  z  z   d|z  z   ||dz  z  z   d|z  |dz  z  z    |                     |¦  «        z  ||z  |z  |z  z
  |d|z  z   d|z  |
dz  z  z    |                     |¦  «        z  ||z  |dz  z  z   dg¦  «        }8|4                     |6|5¦  «        \  }9}:|9                     ¦   «         |7                     ¦   «         k    sJ ‚|8t1          |:¦  «        z
                       ¦   «         t3          dd¦  «        k    sJ ‚d S )Nr©   r)   rª   zu1:4r¬   r­   r®   zQ1 Q2 Q3r¯   r,   r°   r&   r(   r*   r±   r   r²   r³   r´   rµ   r¶   r·   r¸   c                 ó`   •— g | ]*}t          |                     ‰¦  «        ‰j        ¦  «        ‘Œ+S r   rº   r»   s     €€r   r    z!test_sub_qdot.<locals>.<listcomp>W  s/   ø€ Ð
8Ð
8Ð
8 !�3ˆq�uŠu�Q‰xŒx˜œÑÔÐ
8Ð
8Ð
8r"   r½   r¾   r¿   )rR   rW   r+   r.   )r   r	   r   r[   r^   r]   r_   r\   r   ra   rb   rÁ   r   ÚlistÚitemsr   r   r   r   r   r   r   rm   r=   r   r   )=rÇ   rÈ   rÉ   rÊ   rË   rÌ   rÍ   rÎ   rÏ   rÒ   rÓ   rÔ   rs   rÕ   rÖ   r×   rØ   rÙ   rÚ   rÛ   rÜ   ru   rÝ   r   rá   râ   rã   rÞ   rß   rà   r,   r(   r*   rä   rå   ræ   rç   rè   ré   r¼   rê   rë   rì   Úkde_mapÚkÚvrî   rï   rð   r½   r¾   r¿   rñ   rò   ró   Úfr_expectedrö   r¡   rô   r&   r¯   s=                                                              @@r   Útest_sub_qdotr  *  s  øø€ õ
   Ñ'Ô'�J€BˆˆBÝ" 6°Ð3Ñ3Ô3�M€CˆˆcÝ Ñ'Ô'�J€BˆˆBÝ$+Ð,FÑ$GÔ$GÑ!€GˆQ��1�a˜˜EÝ 'Ð(<Ñ =Ô =Ñ€A€qˆ"ˆb�"�a˜˜AÝÐ/Ñ0Ô0Ñ€Dˆ$�Ý˜Ñ$Ô$�J€BˆˆBõ 	�sÑÔ€AØ	�Š�C˜ 5 &¨!¬# Ñ/Ô/€AØ	�Š�C˜ " a¤c Ñ+Ô+€AØ‡M‚M�!�R˜œ‘V˜b ¤™f‘_Ñ%Ô%Ð%à	�Š�C˜ " a¤c Ñ+Ô+€AØ	�Š�C˜ " a¤c Ñ+Ô+€Aõ 
ˆs‰Œ€BØ‡J‚Jˆq�!ÑÔÐà‡J‚Jˆq�"�q”s‘(ÑÔÐà�lŠl˜4  1¤3¡Ñ'Ô'€GØ	�Š�c˜1˜QœS™5 1 Q¤S¡5™=Ñ	)Ô	)€Bà�lŠl˜4  1¤3¡Ñ'Ô'€GØ�lŠl˜4  1¤3¡Ñ'Ô'€GØ�lŠl˜4 !  A¤C¡Ñ(Ô(€GØ�r˜7 G¨WÐ5ð  ð  ˆØ	�Š�b˜!˜QÑÔÐÐð ×Ò˜t a R¨¬¡VÑ,Ô,€FØ×Ò˜t a R¨¬¡VÑ,Ô,€FØ
×Ò�w  1Ñ%Ô%Ð%Ø
×Ò�w  1Ñ%Ô%Ð%ð 9Ð
8Ð
8Ð
8Ð
8¨°Ð'7Ð
8Ñ
8Ô
8€CØˆB�‰Hˆ:Ñ€CÝ�C˜#˜s C˜Ñ)Ô)€GÝ�W—]’]‘_”_Ñ%Ô%ð 'ð '‰ˆˆ1ØŸVšV A™YœYˆ�—’�q‘	”	ÑÐõ ˜˜2˜t T¨1¨d°AÑ6Ô6€IÝ˜˜1˜a Ñ#Ô#€IÝ˜˜1˜a Ñ#Ô#€Iõ �E˜7 A r¨I°wÐ+?Ñ
@Ô
@€CÝ
�E˜7 A r¨I°wÐ+?Ñ
@Ô
@€CÝ
�E˜7 A r¨I°wÐ+?Ñ
@Ô
@€Cõ 
�Q˜˜R ˜ r¨2 h°sÈÈÐ	MÑ	MÔ	M€Bà˜˜˜1™˜QœS™Ð! B¨¨1¬3©°°A´C±©¸"¸Q¼S¹&Ñ(@Ð#AÐB€FØ�3˜ˆ_€Fõ Øˆb‰D�1�Q‘3�q‘5�˜U™œÑ#¥C¨¡G¤GÑ+Ñ+Ø��1‘•S˜‘Z”Z‘¥ B¡¤Ñ'Ñ'Øˆa‰C�‰E•#�e‘*”*Ñ˜r !™tÑ# b¨¡dÑ*ð,ñ -ô -€Kõ
 Ø�1�Q‘3�q˜!‘t‘8˜A˜q™D‘=Ñ  1 Q¡3Ñ&Ø��A‘‰gñØ˜"™˜Q ™T™	ñ"ð #Ø%'§W¢W¨Q¡Z¤Zñ0Ø24°Q±$°r±'¸"±*ñ=à�1�R‘4‰i˜˜1™˜Q ™T™Ñ!Ð" R§W¢W¨Q¡Z¤ZÑ/°"°Q±$°r¸1±u±*Ñ<Øð	ñ ô Ðð ×$Ò$ V¨VÑ4Ô4�K€BˆØ�IŠI‰KŒK˜;×-Ò-Ñ/Ô/Ò/Ð/Ð/Ð/Ø¥¨Ñ 1Ô 1Ñ1×9Ò9Ñ;Ô;½uÀQÈ¹{¼{ÒJÐJÐJÐJÐJÐJr"   c                  ó@	  ‡‡‡ — t          d¦  «        \  } }}}}}}t          d¦  «        }t          dd¬¦  «        }t          d¦  «        }	t          d¦  «        Š‰                     dd|d	         ‰j        g¦  «        }
|
                     d
dt
          dz  |d         z
  |
j        g¦  «        }|                     dd|d         |j        g¦  «        Št          d¦  «        }|                     ‰d	¦  «         | 	                    d|d         ‰j        z  |d         ‰j
        z  z   ¦  «        }|                     ‰|                     |¦  «                             |‰¦  «        ¦  «         |                     |d	¦  «         | 	                    dd	¦  «        Š ‰                      ‰d	¦  «         ‰  	                    d||j
        z  ¦  «        }|                     ‰d	¦  «         |                     |d	¦  «         |                     |‰|¦  «         ‰                      |‰‰¦  «         |‰j        z  |‰j
        z  z   |‰j        z  z   }| | z  ‰j        z  }‰ |f||fg}ˆˆfd„|D ¦   «         }||dd …         z  }d„ t          |	|¦  «        D ¦   «         }t          ‰||	|¦  «        }|                     g |¦  «        \  }}|	d d…         }t#          t%          |	¦  «        t%          |¦  «        z
  ¦  «        }ˆˆ fd„‰j        ‰j
        fD ¦   «         }t          ‰|||||¬¦  «        }|                     g |¦  «        \  }}t'          | | z  |z  t)          |d         ¦  «        z  | |t+          |d	         ¦  «        z  |t)          |d	         ¦  «        z  z   z  t-          |d         ¦  «        z  ||t+          |d	         ¦  «        z  |t)          |d	         ¦  «        z  z   z  ||g¦  «        }t'          | | z  |z  t)          |d         ¦  «        z  d	d	g¦  «        }t/          |                     ¦   «         ¦  «        t/          |                     ¦   «         ¦  «        k    sJ ‚t/          |                     ¦   «         ¦  «        t/          |                     ¦   «         ¦  «        k    sJ ‚d S )Nzg m Px Py Pz R tzq:5r)   rª   zu:5r&   ÚB_primer'   r   r(   r+   r*   r-   rÓ   r.   r/   r¸   r¶   c                 ó@   •— g | ]}‰                      ‰¦  «        |z  ‘ŒS r   )r2   )r   r3   r&   r*   s     €€r   r    z"test_sub_qdot2.<locals>.<listcomp>¶  s(   ø€ Ð/Ð/Ð/ rˆa�lŠl˜1‰oŒo Ñ"Ð/Ð/Ð/r"   c                 ó   — g | ]
\  }}||z
  ‘ŒS r   r   )r   r$   rÕ   s      r   r    z"test_sub_qdot2.<locals>.<listcomp>¸  s    € Ð
.Ð
.Ð
.‘e�b˜!ˆ2�‰6Ð
.Ð
.Ð
.r"   c                 ó@   •— g | ]}‰                      ‰¦  «        |z  ‘ŒS r   )r8   )r   r3   r&   rë   s     €€r   r    z"test_sub_qdot2.<locals>.<listcomp>À  s(   ø€ Ð	2Ð	2Ð	2 ˆ&�*Š*�Q‰-Œ-˜"Ñ
Ð	2Ð	2Ð	2r"   )rU   rV   )r	   r   r   r[   r\   r   r]   r   ra   rb   r^   r<   r   rÁ   rY   r   rm   rÿ   Úsetr   r   r   r   r   r=   )!rs   rr   ÚPxÚPyÚPzrÓ   r   ry   rz   r{   r  r(   ÚpOÚpRÚpCsÚR_C_hatÚR_Csrò   Úu_exprrì   Úkm1Úfr1Ú_Úu_indepr“   rí   Úkm2rú   Úfr1_expectedÚfr2_expectedr&   r*   rë   s!                                 @@@r   Útest_sub_qdot2r  ‚  s„  øøø€ õ %Ð%7Ñ8Ô8Ñ€A€qˆ"ˆb�"�a˜Ý�uÑÔ€AÝ	˜ QÐ	'Ñ	'Ô	'€BÝ�uÑÔ€Aõ 	�sÑÔ€AØ�kŠk˜) V¨a°¬d°A´C¨[Ñ9Ô9€GØ×Ò˜#˜v­¨1©¨q°¬t©°W´YÐ'?Ñ@Ô@€AØ	�Š�C˜ ! A¤$¨¬ Ñ-Ô-€Aõ 
ˆs‰Œ€BØ‡J‚Jˆq�!ÑÔÐð 
�Š�c˜1˜Qœ4 ¤™8 a¨¤d¨1¬3¡hÑ.Ñ	/Ô	/€BØ‡J‚Jˆq�"—+’+˜b‘/”/×&Ò& q¨!Ñ,Ô,Ñ-Ô-Ð-Ø‡J‚Jˆq�!ÑÔÐð �\Š\˜$ Ñ"Ô"€FØ
‡N‚N�1�aÑÔÐð ×
Ò
˜4  1¤3¡Ñ
'Ô
'€CØ‡K‚K��1ÑÔÐØ‡K‚K��1ÑÔÐð ‡O‚O�B˜˜1ÑÔÐØ
×Ò�s˜A˜qÑ!Ô!Ð!ð �”‰f�r˜!œ#‘v‰o  1¤3¡Ñ&€GØˆ2ˆa‰4�”‰8€DØ�wÐ # t Ð-€Fð
 0Ð/Ð/Ð/Ð/¨QÐ/Ñ/Ô/€FØ
ˆb���ŒfÑ€FØ
.Ð
.�s 1 f™~œ~Ð
.Ñ
.Ô
.€CÝ
�a˜˜A˜sÑ
#Ô
#€CØ× Ò   VÑ,Ô,�F€Cˆð ���Œe€GÝ•�Q‘”�#˜g™,œ,Ñ&Ñ'Ô'€EØ	2Ð	2Ð	2Ð	2Ð	2¨¬¨Q¬S zÐ	2Ñ	2Ô	2€BÝ
�a˜˜G SØ"'¸bðBñ Bô B€Cà× Ò   VÑ,Ô,�F€CˆåØ	
ˆˆ1‰ˆQ‰�s�1�Q”4‰yŒyÑØ	
ˆˆB�s�1�Q”4‰yŒy‰L˜2�c ! A¤$™iœi™<Ñ'Ñ(­¨Q¨q¬T©¬Ñ2Ø	ˆ2�c�!�A”$‰iŒi‰<˜"�S  1¤™YœY™,Ñ&Ñ'Ø
Ø
ðñ ô €Lõ Ø	
ˆˆ1‰ˆQ‰�s�1�Q”4‰yŒyÑØ	Ø	ðñ ô €Lõ �S—Z’Z‘\”\Ñ"Ô"¥h¨|×/BÒ/BÑ/DÔ/DÑ&EÔ&EÒEÐEÐEÐEÝ�S—Z’Z‘\”\Ñ"Ô"¥h¨|×/BÒ/BÑ/DÔ/DÑ&EÔ&EÒEÐEÐEÐEÐEÐEr"   N)Úsympyr   r   r   r   r   r   r	   Úsympy.simplify.simplifyr
   Úsympy.simplify.trigsimpr   Úsympy.solvers.solversr   Úsympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r§   rü   r  r  r   r"   r   ú<module>r"     s`  ðØ ;Ð ;Ð ;Ð ;Ð ;Ð ;Ð ;Ð ;Ð ;Ð ;Ð ;Ð ;Ð ;Ð ;Ð ;Ð ;Ð ;Ð ;Ø ,Ð ,Ð ,Ð ,Ð ,Ð ,Ø ,Ð ,Ð ,Ð ,Ð ,Ð ,Ø 'Ð 'Ð 'Ð 'Ð 'Ð 'ð@ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ð @ðy*ð y*ð y*ðxa:ð a:ð a:ðFVLð VLð VLðpNGð NGð NGð NGð NGr"   