Ë
    7^(h8K  ã                   ó”   — 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y
)é    )ÚcosÚMatrixÚsinÚzerosÚtanÚpiÚsymbols)Úsimplify)Útrigsimp)Úsolve)
ÚcrossÚdotÚdynamicsymbolsÚfind_dynamicsymbolsÚKanesMethodÚinertiaÚinertia_of_point_massÚPointÚReferenceFrameÚ	RigidBodyc                  óŠ  — t        d«      \  } }}}}}t        d«      \  }}}t        d«      }	|	D �
cg c]  }
|
j                  | «      ‘Œ }}
t        d«      }|D �cg c]  }|j                  | «      ‘Œ }}t        t	        |dgt        |«      z  «      «      }t        d«      }t        t	        |dgt        |«      z  «      «      }t        d«      }|j                  dd	|	d
   |j                  g«      }|j                  dd	|	d   |j                  g«      }|j                  dd	|	d   |j                  g«      }|j                  ||d
   |j                  z  |d   |j                  z  z   |d   |j                  z  z   «       |j                  ||j                  |«      j                  | |«      t        |j                  |«      |j                  |«      «      z   «       t        d«      }|j!                  ||d
   |j                  z  |d   |j                  z  z   |d   |j                  z  z   «       |j#                  d|	d   |j                  z  |t%        |	d   «      z  |j                  z  z   «      }|j!                  ||d   |j                  z  |d   |j                  z  z   |d   |j                  z  z   «       |j'                  ||j)                  |«      j                  | |«      t        |j                  |«      |j)                  |«      «      z   «       |d
   |j                  z  |d   |j                  z  z   |d   |j                  z  z   }|j+                  |«      j                  | |«      t        |j                  |«      |j+                  |«      «      z   }t-        |D �cg c]   }t/        ||j                  |«      z
  |«      ‘Œ" c}t/        ||j)                  |«      z
  |j                  «      gz   «      }t1        ||«      }t1        |j3                  |d   d
|d   d
i«      |«      }|j5                  |d   d
|d   d
i«       ||z   D �cg c]#  }|j                  |«      j                  ||«      ‘Œ% }}||z   D �cg c]#  }|j)                  |«      j                  ||«      ‘Œ% }}||z   D �cg c]#  }|j)                  |«      j                  ||«      ‘Œ% } }t-        t/        | |j                  z  |j                  «      |	d   z
  g«      }!t-        |D �"cg c]h  }"t/        |j)                  |«      |j)                  |«      t        |j                  |«      |j+                  |«      «      z   z
  |"«      j7                  «       ‘Œj c}"«      }#t        |j                  |«      |j+                  |«      «      }$|$j                  | |«      t        |j                  |«      |$«      z   }%t-        |D �"cg c]   }"t/        |j9                  |«      |%z
  |"«      ‘Œ" c}"«      }&t;        dd«      }'t=        d«      D ]1  }(t?        ||z   «      D ]  \  })}|#|(   j                  |«      |'|(|)f<   Œ  Œ3 |'d d …d d…f   }*|'d d …dd…f   }+|'d d …dd …f   },|*jA                  |,«      }-|+jC                  «        |-z  }.|.d d …d d…f   t-        |d d «      z  }/t        t	        |dd  |/«      «      }0||z  |j                  z  }1||j                  z  ||j                  z  z   ||j                  z  z   }2t-        t	        || «      D �3�4cg c]  \  }3}4t/        |1|3«      t/        |2|4«      z   ‘Œ  c}4}3«      }5| |j9                  |«      z  }6tE        ||||«      }7t/        |7|jG                  |«      «      t        |j                  |«      t/        |7|j                  |«      «      «      z    }8t-        t	        ||«      D �9�:cg c]  \  }9}:t/        |6|9«      t/        |8|:«      z   ‘Œ  c}:}9«      };|5d d…d d …f   jI                  |5dd …d d …f   «      |.jJ                  |5dd…d d …f   z  z   }<|;d d…d d …f   jI                  |;dd …d d …f   «      |.jJ                  |;dd…d d …f   z  z   }=|=j3                  |«      j3                  |0«      j3                  |«      j3                  |	d   | tM        |	d   «      z  i«      j7                  «       }>|7|f}?tO        d||||?«      }@|@g}A||1f}B||2f}C|B|Cg}DtQ        ||	d d |d d ||	dd  |!|dd  |#|¬«	      }E|EjS                  |A|D«      \  }F}G|Gj3                  |«      j3                  |0«      j3                  |«      j3                  |	d   | tM        |	d   «      z  i«      j7                  «       }H|EjU                  «       }It-        |<«      j7                  «       |Fj7                  «       k(  sJ ‚t-        |=j3                  I«      «      j7                  «       Gj7                  «       k(  sJ ‚tW        t-        |>«      j7                  «       «      jY                  d
di«      tW        Hj7                  «       «      jY                  d
di«      k(  sJ ‚t[        Ej\                  «      }J|D ]  }K|KJvrŒJ ‚ y c c}
w c c}w c c}w c c}w c c}w c c}w c c}"w c c}"w c c}4}3w c c}:}9w )Nzt r m g I JzFx Fy Fzzq:4zu:6g        zua:3ÚNÚAÚAxisr   ÚBé   ÚCé   ÚPÚOé   é   é   é	   é   Údisc)Úq_indÚu_indÚkd_eqsÚq_dependentÚconfiguration_constraintsÚu_dependentÚvelocity_constraintsÚu_auxiliary)/r	   r   ÚdiffÚdictÚzipÚlenr   Ú	orientnewÚzÚxÚyÚset_ang_velÚset_ang_accÚ
ang_vel_inr   r   Úset_velÚ	locatenewr   Úset_accÚvelÚpos_fromr   r   r   ÚsubsÚupdateÚexpandÚaccr   ÚrangeÚ	enumerateÚrow_joinÚinvr   Ú
ang_acc_inÚcol_joinÚTr   r   r   Úkanes_equationsÚkindiffdictr
   Úxreplacer   Úforcing)LÚtÚrÚmÚgÚIÚJÚFxÚFyÚFzÚqÚqiÚqdÚuÚuiÚudÚud_zeroÚuaÚua_zeror   r   r   r   r   r    Úw_c_n_qdÚv_o_n_qdÚuvÚkindiffsÚqd_kdÚsteady_conditionsÚpartial_w_CÚpartial_v_OÚpartial_v_PÚf_cÚaiÚf_vÚv_o_nÚa_o_nÚf_aÚM_vÚiÚjÚM_v_iÚM_v_dÚM_v_auxÚ	M_v_i_auxÚA_rsÚu_depÚ
u_dep_dictÚF_OÚF_PÚpv_oÚpv_pÚFr_uÚR_star_OÚI_C_OÚT_star_CÚpvÚpavÚ	Fr_star_uÚFr_cÚ	Fr_star_cÚFr_star_steadyÚ
iner_tupler&   ÚbodyListÚF_oÚF_pÚ	forceListÚkaneÚfrÚfrstarÚfrstar_steadyÚkddÚsyms_in_forcingÚqdisL                                                                               úf/var/www/skyplay_api_hub/venv/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_kane2.pyÚtest_aux_depr”      sÝ	  € ô" ˜}Ó-Ñ€A€qˆ!ˆQ��1Ü˜Ó$�J€BˆˆBô& 	�uÓ€AØ Ö	!˜ˆ"�'‰'�!�*Ð	!€BÐ	!Ü�uÓ€AØ Ö	!˜ˆ"�'‰'�!�*Ð	!€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¡S¡Ñ(¨1¨Q©4°·±©8Ñ3Ô4Ø‡M�M�!�Q—\‘\ !“_×)Ñ)¨!¨QÓ/Ü˜1Ÿ<™<¨›?¨A¯L©L¸«OÓ<ñ=ô >ô 	ˆc‹
€AØ‡I�Iˆa��A‘�q—s‘s‘˜R ™U 1§3¡3™YÑ&¨¨A©¨q¯s©s©Ñ2Ô3Ø	�‰�C˜˜1™˜aŸc™c™ A¤c¨!¨A©$£i¡K°·±¡OÑ3Ó4€AØ‡I�Iˆa��1‘�a—c‘c‘˜A˜a™D §¡™HÑ$ q¨¡t¨A¯C©C¡xÑ/Ô0Ø‡I�Iˆa�—‘�q“—‘˜q !Ó$¤u¨Q¯\©\¸!«_¸a¿e¹eÀA»hÓ'GÑGÔHð �!‰u�Q—S‘S‰y˜2˜a™5 §¡™9Ñ$ r¨!¡u¨Q¯S©S¡yÑ0€HØ�z‰z˜!‹}×!Ñ! ! QÓ'¬%°·±¸Q³ÀÇÁÈAÃÓ*OÑO€HÜÀQÖG¸r”s˜8 a§l¡l°1£oÑ5°rÕ:ÒGÜ˜8 a§e¡e¨A£hÑ.°·±Ó4Ð5ñ6ó 7€Hä�(˜BÓ€Eô ˜hŸm™m¨R°©U°Q¸¸1¹ÀÐ,BÓCÀQÓGÐØ×Ñ˜b ™e a¨¨A©°Ð3Ô4ð :;¸R¹Ö@°2�1—<‘< “?×'Ñ'¨¨AÕ.Ð@€KÐ@Ø23°b±&Ö9¨B�1—5‘5˜“8—=‘=  QÕ'Ð9€KÐ9Ø23°b±&Ö9¨B�1—5‘5˜“8—=‘=  QÕ'Ð9€KÐ9ô ”#�q�b˜Ÿ™‘f˜aŸc™cÓ" Q q¡TÑ)Ð*Ó
+€CÜ
Ø01ö3Ø*,ô �a—e‘e˜A“h !§%¡%¨£(¬U°1·<±<À³?Ø	�
‰
�1‹ó.ñ #ñ Øóß#™V›Xñ&ò 3ó 4€Cä�!—,‘,˜q“/ 1§:¡:¨a£=Ó1€EØ�J‰J�q˜!Óœu Q§\¡\°!£_°eÓ<Ñ<€EÜ
°aÖ8°”#�a—e‘e˜A“h Ñ&¨Õ+Ò8Ó
9€Cô ��1‹+€CÜ�1‹Xò (ˆÜ˜q 2™vÓ&ò 	(‰EˆAˆrØ˜A™Ÿ™ B›ˆC��1�ŠIñ	(ð(ð ’�2�A�2�‰J€EØ’�1�Q�3�‰K€EØ’!�Q‘R�%‰j€GØ—‘˜wÓ'€IØ�Y‰Y‹[ˆ=˜9Ñ$€Dà’�B�Q�B�‰Kœ&  2 A ›-Ñ'€EÜ”c˜!˜A˜B˜% Ó'Ó(€Jð ˆA‰#ˆa�c‰c‰'€CØ
ˆq�s‰s‰(�R˜!Ÿ#™#‘XÑ
  Q§S¡S¡Ñ
(€CÜÜ�˜[Ó)÷+±z°t¸T”3�s˜D“>¤C¨¨T£NÓ2ó +ó ,€Dð ˆr�!—%‘%˜“(‰{€HÜ�A�q˜!˜QÓ€EÜ�U˜AŸL™L¨›OÓ,Ü˜Ÿ™ a›¬#¨e°Q·\±\À!³_Ó*EÓFñGð H€HäÜ˜K¨Ó5÷7Á7À2Àsœ˜H bÓ)¬C°¸#Ó,>Ó>ó 7ó 8€Ið
 ���’A�‰;×Ñ  Q¡Rª U¡Ó,¨t¯v©v¸¸Q¸q¸SÂ!¸V¹Ñ/DÑD€DØ˜"˜1˜"ša˜%Ñ ×)Ñ)¨)°A±Bº°EÑ*:Ó;Ø—&‘&˜9 Q q Sª! VÑ,Ñ,ñ-€Ià—^‘^ GÓ,×1Ñ1°*Ó=ß‰TÐ#Ó$§T¡T¨1¨Q©4°!°´C¸¸!¹³I±Ð*>Ó%?ÇÁÃð ð ˜�€JÜ�V˜Q  1 jÓ1€DØˆv€Hð ˆcˆ(€CØˆcˆ(€CØ�s�€Iô Ø	�!�B�Q�%  " 1 ¨hØ�a�b�E°sØ�a�b�E°Øô	
€Dð ×&Ñ& x°Ó;�L€RˆØ—K‘K Ó(×-Ñ-¨jÓ9×>Ñ>Ð?PÓQß‘T˜1˜Q™4 ! ¤C¨¨!©£I¡Ð.Ó/·±³ð à
×
Ñ
Ó
€Cä�$‹<×ÑÓ  B§I¡I£KÒ/Ð/Ð/Ü�)—.‘. Ó%Ó&×-Ñ-Ó/°6·=±=³?ÒBÐBÐBô ”V˜NÓ+×2Ñ2Ó4Ó5×>Ñ>ÀÀ#¸wÓGÜ�]×)Ñ)Ó+Ó,×5Ñ5°q¸°gÓ>ò?ð @ð ?ô *¨$¯,©,Ó7€OØò *ˆØ˜/Ò)Ð)Ð)ñ*ùòe 
"ùâ	!ùòJ Hùò AùÚ9ùÚ9ùò3ùò 9ùó6+ùó7s<   ±jÁjÎ%jÑ(j Ñ7(j%Ò((j*ÔA-j/×$%j4Ü(#j9
ß#j?
c                  ó<  — t        d«      \  } }}t        dd¬«      \  }}}t        d«      \  }}}}	}
t        d«      \  }}}}}}}t        d«      \  }}}}}}}}t        d«      \  }}}t        d«      \  }}}t        d	«      } | j                  d
d| | j                  g«      }!|!j                  dd| |!j
                  g«      }"|"j                  | ||"j
                  z  ||"j                  z  z   «       |"j                  dd||"j                  g«      }#|"j                  dd||"j                  g«      }$|#j                  |"|	|"j                  z  «       |$j                  |"|
|"j                  z  «       t        d«      }%|%j                  |"d«       |%j                  | ||"j                  z  «       |%j                  d||"j                  z  «      }&|%j                  d||"j                  z  ||"j
                  z  z
  «      }'|&|'fD ]  }(|(j                  |%| |"«       Œ |%j                  d||"j                  z  «      })|%j                  d||"j                  z  «      }*|%j                  d| |"j                  z  «      }+|)|*|+fD ]  }(|(j                  |%| |"«       Œ |*j                  d| |"j
                  z  «      },|+j                  d| |"j
                  z  «      }-|,j                  |*| |#«       |-j                  |+| |$«       ||z
  ||	z
  ||
z
  g}.|,|-fD �(cg c]'  }(t        |(j                  | «      |"j                  «      ‘Œ) }/}(t        |"|||d|d«      }0t        |#|||«      }1t        |$|||«      }2t        d|)|"||0|)f«      }3t        d|*|#||1|*f«      }4t        d|+|$||2|+f«      }5t!        | | ||g||g|.|	|
g|/|g¬«      }6|&| |z  | j
                  z  f|'||"j
                  z  ||"j                  z  z   ||"j                  z  z   fg}7|3|4|5g}8|6j#                  |8|7«      \  }9}:t%        |/|	|
g«      };t'        |d|z  |dz  z  |dz  z  z   d|z  z   ||dz  z  z   d|z  |dz  z  z    |j)                  |«      z  ||z  |z  |z  z
  |d|z  z   d|z  |dz  z  z    |j)                  |«      z  ||z  |dz  z  z   dg«      }<t+        |:j-                  |;«      j-                  |di«      «      j/                  «       j1                  «       }|<|z
  j1                  «       t3        dd«      k(  sJ ‚g }=t5        |3|4|5g|0|1|2g«      D ]€  \  }>}?|?t7        |>j8                  |>j:                  j=                  |%«      |>j>                  «      z   }@|=jA                  t        d|>j:                  |>j>                  |>j8                  |@|%f«      «       Œ‚ |6j#                  |=|7«      \  }A}Bt+        |Bj-                  |;«      j-                  |di«      «      j/                  «       }|<|z
  j1                  «       t3        dd«      k(  sJ ‚y c c}(w )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^ÚrbAÚrbBÚrbC)r'   r(   r)   r,   r-   r.   r   r!   Ú )!r   r	   r   r3   r6   r5   r7   r4   r   r:   r;   Úv2pt_theoryr   r=   r   r   r   rJ   r   r   r/   r   r?   ÚdoitrA   r   r1   r   ÚmassÚ
masscenterr>   ÚframeÚappend)CÚq1Úq2Úq3Úq1dÚq2dÚq3dÚu1Úu2Úu3Úu4Úu5Úu_primeÚRÚMrQ   ÚeÚfÚthetaÚaÚbÚmAÚmBÚIArS   ÚKrN   ÚQ1ÚQ2ÚQ3ÚIA22ÚIA23ÚIA33rœ   r   r   r   r   ÚpDÚpS_starÚpQÚpÚ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_starrR   Úfr2Úfr_star2sC                                                                      r“   Útest_non_central_inertiaræ   Ç   s/  € ô   Ó'�J€BˆˆBÜ" 6°Ô3�M€CˆˆcÜ'¨Ó/Ñ€BˆˆB��BÜ$+Ð,FÓ$GÑ!€GˆQ��1�a˜˜EÜ 'Ð(<Ó =Ñ€A€qˆ"ˆb�"�a˜˜AÜ˜Ó%�J€BˆˆBÜÐ/Ó0Ñ€Dˆ$�ô 	�sÓ€AØ	�‰�C˜ 5 &¨!¯#©# Ó/€AØ	�‰�C˜ " a§c¡c Ó+€AØ‡M�M�!�R˜Ÿ™‘V˜b §¡™f‘_Ô%à	�‰�C˜ " a§c¡c Ó+€AØ	�‰�C˜ " a§c¡c Ó+€AØ‡M�M�!�R˜!Ÿ#™#‘XÔØ‡M�M�!�R˜!Ÿ#™#‘XÔô 
ˆs‹€BØ‡J�Jˆq�!Ôà‡J�Jˆq�"�q—s‘s‘(Ôà�l‰l˜4  1§3¡3¡Ó'€GØ	�‰�c˜1˜QŸS™S™5 1 Q§S¡S¡5™=Ó	)€BØ�rˆ]ò  ˆØ	�‰�b˜!˜QÕð ð �l‰l˜4  1§3¡3¡Ó'€GØ�l‰l˜4  1§3¡3¡Ó'€GØ�l‰l˜4 !  A§C¡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Ø'-¨vÐ&6Ö	7 Œ#ˆa�e‰e�A‹h˜Ÿ™Õ
Ð	7€BÐ	7ô ˜˜2˜t T¨1¨d°AÓ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™S™Ð! B¨¨1¯3©3©°°A·C±C±©¸"¸Q¿S¹S¹&Ñ(@Ð#AÐB€FØ�3˜ˆ_€FØ×$Ñ$ V¨VÓ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ñ0Ø24°Q±$°r±'¸"±*ñ=à�1�R‘4‰i˜˜1™˜Q ™T™Ñ!Ð" R§W¡W¨Q£ZÑ/°"°Q±$°r¸1±u±*Ñ<Øð	ó Ðô
 	�—‘˜fÓ%×*Ñ*¨B°¨7Ó3Ó4×9Ñ9Ó;×BÑBÓD€AØ Ñ!×)Ñ)Ó+¬u°Q¸«{Ò:Ð;Ð:ð €GÜ˜3  S˜/¨I°yÀ)Ð+LÓMò +‰
ˆˆFØÔ*¨2¯7©7Ø+-¯=©=×+AÑ+AÀ"Ó+EØ+-¯8©8ó5ñ 5ˆð 	�‰”y  R§]¡]°B·H±H¸b¿g¹gØ"# R ó*õ 	+ð	+ð ×&Ñ& w°Ó7�M€Cˆä�—‘˜vÓ&×+Ñ+¨R°¨GÓ4Ó5×:Ñ:Ó<€AØ˜qÑ ×(Ñ(Ó*¬e°A°q«kÒ9Ð9Ñ9ùò] 
8s   Ì,Xc            	      ó˜
  — t        d«      \  } }}t        dd¬«      \  }}}t        d«      \  }}}t        d«      \  }	}
}}}}}t        d«      \  }}}}}}}}t        d«      \  }}}t        d«      \  }}}t        d	«      }|j                  d
d| |j                  g«      }|j                  dd| |j
                  g«      } | j                  ||| j
                  z  || j                  z  z   «       | j                  dd|| j                  g«      }!| j                  dd|| j                  g«      }"t        d«      }#|#j                  | d«       |#j                  ||| j                  z  «       |#j                  d|| j                  z  «      }$|#j                  d|| j                  z  |
| j
                  z  z
  «      }%|#j                  d|| j                  z  «      }&|#j                  d|| j                  z  «      }'|#j                  d| | j                  z  «      }(|$|%|&|'|(fD ]  })|)j                  |#|| «       Œ |'j                  d|
 | j
                  z  «      }*|(j                  d|
 | j
                  z  «      }+|*j                  |'||!«       |+j                  |(||"«       |*|+fD �)cg c]'  })t        |)j                  |«      | j                  «      ‘Œ) },})|,||z
  gz  },t        |,|||g«      }-t        |-j!                  «       «      D ](  \  }.}/|/j#                  |«      |-|.j#                  |«      <   Œ* t%        | |||d|d«      }0t%        |!|||«      }1t%        |"|||«      }2t'        d|&| ||0|&f«      }3t'        d|'|!||1|'f«      }4t'        d|(|"||2|(f«      }5t)        || ||g||g|,|g¬«      }6|$| |z  |j
                  z  f|%|| j
                  z  || j                  z  z   || j                  z  z   fg}7|3|4|5g}8t+        ||z  ||z  |z  t-        |«      z  t/        | «      z  z   |||z  t-        |«      z  t-        | «      z  z   ||z  |z  t/        |«      z  ||z  z
  ||
z  z
  g«      }9t+        |d|z  |dz  z  |
dz  z  z   d|z  z   ||dz  z  z   d|z  |dz  z  z    |j#                  |«      z  ||z  |z  |z  z
  |d|z  z   d|z  |
dz  z  z    |j#                  |«      z  ||z  |dz  z  z   dg«      }:|6j1                  |8|7«      \  };}<|;j3                  «       |9j3                  «       k(  sJ ‚|:t5        |<«      z
  j3                  «       t7        dd«      k(  sJ ‚y c c})w )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¥   r¦   r§   r¨   )r)   r.   r   r!   )r   r	   r   r3   r6   r5   r7   r4   r   r:   r;   rª   r   r=   r   ÚlistÚitemsr/   r   r   r   r   r   r   rJ   rA   r   r   )=r°   r±   r²   r³   r´   rµ   r¶   r·   r¸   r»   r¼   r½   rQ   r¾   r¿   rÀ   rÁ   rÂ   rÃ   rÄ   rÅ   rS   rÆ   rN   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Þ   s=                                                                r“   Útest_sub_qdotrî   *  sw  € ô
   Ó'�J€BˆˆBÜ" 6°Ô3�M€CˆˆcÜ Ó'�J€BˆˆBÜ$+Ð,FÓ$GÑ!€GˆQ��1�a˜˜EÜ 'Ð(<Ó =Ñ€A€qˆ"ˆb�"�a˜˜AÜÐ/Ó0Ñ€Dˆ$�Ü˜Ó$�J€BˆˆBô 	�sÓ€AØ	�‰�C˜ 5 &¨!¯#©# Ó/€AØ	�‰�C˜ " a§c¡c Ó+€AØ‡M�M�!�R˜Ÿ™‘V˜b §¡™f‘_Ô%à	�‰�C˜ " a§c¡c Ó+€AØ	�‰�C˜ " a§c¡c Ó+€Aô 
ˆs‹€BØ‡J�Jˆq�!Ôà‡J�Jˆq�"�q—s‘s‘(Ôà�l‰l˜4  1§3¡3¡Ó'€GØ	�‰�c˜1˜QŸS™S™5 1 Q§S¡S¡5™=Ó	)€Bà�l‰l˜4  1§3¡3¡Ó'€GØ�l‰l˜4  1§3¡3¡Ó'€GØ�l‰l˜4 !  A§C¡C¡Ó(€GØ�r˜7 G¨WÐ5ò  ˆØ	�‰�b˜!˜QÕð ð ×Ñ˜t a R¨¯©¡VÓ,€FØ×Ñ˜t a R¨¯©¡VÓ,€FØ
×Ñ�w  1Ô%Ø
×Ñ�w  1Ô%ð )/°Ð'7Ö
8 !Œ3ˆq�u‰u�Q‹x˜Ÿ™ÕÐ
8€CÐ
8ØˆB�‰Hˆ:Ñ€CÜ�C˜#˜s C˜Ó)€GÜ�W—]‘]“_Ó%ò '‰ˆˆ1ØŸV™V A›Yˆ�—‘�q“	Òð'ô ˜˜2˜t T¨1¨d°AÓ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€Bà˜˜˜1™˜QŸS™S™Ð! B¨¨1¯3©3©°°A·C±C±©¸"¸Q¿S¹S¹&Ñ(@Ð#AÐB€FØ�3˜ˆ_€Fô Øˆb‰D�1�Q‘3�q‘5œ˜U›Ñ#¤C¨£GÑ+Ñ+Ø��1‘”S˜“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ñ0Ø24°Q±$°r±'¸"±*ñ=à�1�R‘4‰i˜˜1™˜Q ™T™Ñ!Ð" R§W¡W¨Q£ZÑ/°"°Q±$°r¸1±u±*Ñ<Øð	ó Ðð ×$Ñ$ V¨VÓ4�K€BˆØ�I‰I‹K˜;×-Ñ-Ó/Ò/Ð0Ð/Ø¤¨Ó 1Ñ1×9Ñ9Ó;¼uÀQÈ»{ÒJÐKÑJùòS 9s   Ê,Uc                  óT  — t        d«      \  } }}}}}}t        d«      }t        dd¬«      }t        d«      }	t        d«      }
|
j                  dd|d	   |
j                  g«      }|j                  d
dt
        dz  |d   z
  |j                  g«      }|j                  dd|d   |j                  g«      }t        d«      }|j                  |
d	«       |j                  d|d   |
j                  z  |d   |
j                  z  z   «      }|j                  |
|j                  |«      j                  ||
«      «       |j                  |d	«       |j                  dd	«      }|j                  |d	«       |j                  d||j                  z  «      }|j                  |d	«       |j                  |d	«       |j                  ||
|«       |j                  ||
|«       ||
j                  z  ||
j                  z  z   ||
j                  z  z   }| | z  |
j                  z  }||f||fg}|D �cg c]  }|j                  |
«      |z  ‘Œ }}||dd  z  }t        |	|«      D ��cg c]
  \  }}||z
  ‘Œ }}}t!        |
||	|«      }|j#                  g |«      \  }}|	d d }t%        t'        |	«      t'        |«      z
  «      }|
j                  |
j                  fD �cg c]  }|j)                  |
«      |z  ‘Œ }}t!        |
|||||¬«      } | j#                  g |«      \  }!}t+        | | z  |z  t-        |d   «      z  | |t/        |d	   «      z  |t-        |d	   «      z  z   z  t1        |d   «      z  ||t/        |d	   «      z  |t-        |d	   «      z  z   z  ||g«      }"t+        | | z  |z  t-        |d   «      z  d	d	g«      }#t3        |j5                  «       «      t3        |"j5                  «       «      k(  sJ ‚t3        |!j5                  «       «      t3        |#j5                  «       «      k(  sJ ‚y c c}w c c}}w c c}w )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£   )r,   r-   )r	   r   r   r3   r4   r   r5   r   r:   r;   r6   r>   r/   rª   r9   r1   r   rJ   rè   Úsetr=   r   r   r   r   r   rA   )$rQ   rP   ÚPxÚPyÚPzr¼   rN   rW   rY   rZ   r   rð   r   r   ÚpOÚpRrÕ   ÚpCsÚR_C_hatÚR_CsrÜ   rb   Úu_exprr[   r¾   rÖ   Úkm1Úfr1Ú_Úu_indeprw   r×   Úkm2rä   Úfr1_expectedÚfr2_expecteds$                                       r“   Útest_sub_qdot2r  ‚  sÿ  € ô %Ð%7Ó8Ñ€A€qˆ"ˆb�"�a˜Ü�uÓ€AÜ	˜ QÔ	'€BÜ�uÓ€Aô 	�sÓ€AØ�k‰k˜) V¨a°©d°A·C±C¨[Ó9€GØ×Ñ˜#˜v¬¨1©¨q°©t©°W·Y±YÐ'?Ó@€AØ	�‰�C˜ ! A¡$¨¯© Ó-€Aô 
ˆs‹€BØ‡J�Jˆq�!Ôð 
�‰�c˜1˜Q™4 §¡™8 a¨¡d¨1¯3©3¡hÑ.Ó	/€BØ‡J�Jˆq�"—+‘+˜b“/×&Ñ& q¨!Ó,Ô-Ø‡J�Jˆq�!Ôð �\‰\˜$ Ó"€FØ
‡N�N�1�aÔð ×
Ñ
˜4  1§3¡3¡Ó
'€CØ‡K�K��1ÔØ‡K�K��1Ôð ‡O�O�B˜˜1ÔØ
×Ñ�s˜A˜qÔ!ð �—‘‰f�r˜!Ÿ#™#‘v‰o  1§3¡3¡Ñ&€GØˆ2ˆa‰4�—‘‰8€DØ�wÐ # t Ð-€Fð
 ./Ö/ rˆa�l‰l˜1‹o Ó"Ð/€FÐ/Ø
ˆb��ˆfÑ€FÜ" 1 f›~×
.‘e�b˜!ˆ2�‹6Ð
.€CÑ
.Ü
�a˜˜A˜sÓ
#€CØ× Ñ   VÓ,�F€Cˆð ��ˆe€GÜ”�Q“œ#˜g›,Ñ&Ó'€EØ()¯©¨Q¯S©S zÖ	2 ˆ&�*‰*�Q‹-˜"Ó
Ð	2€BÐ	2Ü
�a˜˜G SØ"'¸bôB€Cà× Ñ   VÓ,�F€CˆäØ	
ˆˆ1‰ˆQ‰Œs�1�Q‘4‹yÑØ	
ˆˆBŒs�1�Q‘4‹y‰L˜2œc ! A¡$›i™<Ñ'Ñ(¬¨Q¨q©T«Ñ2Ø	ˆ2Œc�!�A‘$‹i‰<˜"œS  1¡›Y™,Ñ&Ñ'Ø
Ø
ðó €Lô Ø	
ˆˆ1‰ˆQ‰Œs�1�Q‘4‹yÑØ	Ø	ðó €Lô �S—Z‘Z“\Ó"¤h¨|×/BÑ/BÓ/DÓ&EÒEÐFÐEÜ�S—Z‘Z“\Ó"¤h¨|×/BÑ/BÓ/DÓ&EÒEÐFÑEùò5 0ùã
.ùò 
3s   È"PÉPË
P%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“   ú<module>r
     sF   ðß ;× ;Ñ ;Ý ,Ý ,Ý '÷@÷ @÷ @òy*òxa:òFVLópNGr	  