
    shK                     R   S SK r S SKJr  S SKJr  S SKJrJr  S SKJ	r	J
r
  S SKJr  S SKJrJrJrJrJrJrJrJrJrJrJrJrJr  S SKJr  S S	KJr  \R>                  r \" S
5      r!\" S
S5      r"\" S5      r#\" S5      r$ " S S5      r% " S S\%5      r& " S S\%5      r' " S S5      r(g)    N)symbols)sympify)cossin)eyezeros)ImmutableMatrix)ForceKanesMethodLagrangesMethodParticlePinJointPointPrismaticJointReferenceFrame	RigidBodyTorqueTorqueActuatorSystemdynamicsymbols)simplify)solvezq:6   u:6zua:3c                       \ rS rSr\R
                  " 5       S 5       rSS jrSS jr\R
                  " 5       S 5       r	\R
                  " 5       S 5       r
SS jr\R
                  " 5       S 5       rS	rg
)TestSystemBase   c                 J    [        [        S5      [        S5      5      U l        g )Nframefixed_point)r   r   r   systemselfs    ڃ/Users/tiagomarins/Projetos/claudeai/copy_bank/venv/lib/python3.13/site-packages/sympy/physics/mechanics/tests/test_system_class.py_empty_system_setup"TestSystemBase._empty_system_setup   s    ^G4eM6JK    c                    SnSnU H)  nXA;  d  M
  [        U R                  U5      S S  / :X  a  M)   e   U H&  nXA;  d  M
  [        U R                  U5      S:X  a  M&   e   SU;  a  U R                  R                  b   eg g )N)
q_indq_depqu_indu_depuu_auxkdesholonomic_constraintsnonholonomic_constraints)loadsbodiesjoints	actuators 
eom_method)getattrr!   r8   )r#   excludematricestuplesattrs        r$   _empty_system_check"TestSystemBase._empty_system_check   s    Q;D"t{{D1!4:::  D"t{{D1R777  w&;;))111 'r'   c           
      L   [        [        S5      [        S5      5      U l        U(       a  [	        S5      O[
        n[        S[        S9U l        [        SU R                  S   U R                  S   [        S   US   5      [        S	U R                  S   U R                  S
   [        S   US   5      [        SU R                  S
   U R                  S   [        S
   US
   5      4U l        U R                  R                  " U R                  6   U R                  R                  [        S   S/S9  U R                  R                  US   SS9  U(       aR  U R                  R!                  US   [
        S   -
  5        U R                  R#                  [$        S   [$        S   5        U R                  R'                  [        S
   [        S   -
  [        S   -   5        U R                  R)                  US   [
        S   -
  US
   -   5        US S
 U R                  l        US
S U R                  l        U R                  R.                  S S  U R                  R0                  S S  sU l        U l        U R                  R*                  S S  U R                  R,                  S S  sU l        U l        U R                  R2                  S S  U l        U R                  R4                  S S  U l        U R                  R8                  S S  U l        U R                  R<                  S S  U l        g )Nr   r    r   zrb1:5clsJ1r   r   J2   J3   F)independent   ) r   r   r   r!   r   qdr   r   r4   r   r+   r   r5   
add_jointsadd_coordinates
add_speedsadd_kdesadd_auxiliary_speedsuaadd_holonomic_constraintsadd_nonholonomic_constraintsr,   r-   r)   r*   r0   r1   hcvelocity_constraintsvcr2   nhc)r#   with_speedsr.   s      r$   _create_filled_system$TestSystemBase._create_filled_system(   sj   ^G4eM6JK%0N5!bg95T4;;q>4;;q>1Q41F4QQ1qtLT4;;q>4;;q>1Q41F

 	,##AaDug#>qt7KK  11.KK,,RUBqE:--adQqTkAaD.@A0011!1DEbqEaF!%!2!21!5t{{7H7H7K
DJ!%!2!21!5t{{7H7H7K
DJKK$$Q'	++33A6++2215;;77:r'   c                 "    U R                  SS9  g )NTrW   rX   r"   s    r$   _filled_system_setup#TestSystemBase._filled_system_setupB   s    ""t"4r'   c                 "    U R                  SS9  g )NFr[   r\   r"   s    r$   _filled_system_setup_no_speeds-TestSystemBase._filled_system_setup_no_speedsF   s    ""u"5r'   c                    SU;   d&  U R                   R                  S S  [        S S :X  d   eSU;   d'  U R                   R                  S S  [        S   /:X  d   eSU;   d&  U R                   R                  S S  [        S S :X  d   eSU;   d&  U R                   R                  S S  [
        S S :X  d   eSU;   d&  U R                   R                  S S  [
        SS :X  d   eS	U;   d&  U R                   R
                  S S  [
        S S :X  d   eS
U;   d&  U R                   R                  S S  [        S S :X  d   eSU;   dN  U R                   R                  S S  [        [
        S S [        S S 5       VVs/ s H	  u  p#X#-
  PM     snn:X  d   eSU;   d;  U R                   R                  S S  [        S   [        S   -
  [        S   -   /:X  d   eSU;   d;  U R                   R                  S S  [
        S   [        S   -
  [
        S   -   /:X  d   eSU;   dW  U R                   R                  S S  [        S   [        S   -
  [        S   -   [
        S   [        S   -
  [
        S   -   /:X  d   eSU;   d/  U R                   R                  [!        U R                  5      :X  d   eSU;   d0  U R                   R"                  [!        U R"                  5      :X  d   eg g s  snnf )Nr)   rG   r*   r+   rI   r,   rE   r-   r.   r/   r0   r1   r   r   r2   rT   r4   r5   )r!   r)   r+   r*   r,   r.   r-   r/   rP   r0   ziprJ   r1   r2   rT   r4   tupler5   )r#   r:   uiqdis       r$   _filled_system_check#TestSystemBase._filled_system_checkJ   s   '!T[[%6%6q%9QrU%BBB'!T[[%6%6q%9adV%CCCg~q!1QrU!:::'!T[[%6%6q%9QrU%BBB'!T[[%6%6q%9QqV%CCCg~q!1QrU!:::'!T[[%6%6q%9RV%CCC DKK$4$4Q$7$'"1r"1v$6<8$6BH$6<8 %8 	8 8'7211!41!qt9K8LL	N M*g544Q7AaD2a5L1Q4<O;PP	 Q ''1003qEBqEMBqE)1Q4"Q%<!A$+>8@ @	A @ G#""eDKK&88	: 9G#""eDKK&88	: 98 $<8s   1Kc                    [         S   U R                  l        [        S   U R                  l        [        S   [         S   R                  [        5      -
  U R                  l        [        S[        S5      S9nU R                  R                  U5        UR                  R                  U R                  R                  [         S   U R                  R                  -  5        g )Nr   pmmass)r+   r!   r)   r.   r,   difftr0   r   r   
add_bodies
masscenterset_posr    x)r#   r%   rj   s      r$   _moving_point_mass!TestSystemBase._moving_point_massa   s    aDaDQ4!A$))A,.Sws|,q!	T[[44adT[[]]6JKr'   )r4   rS   r5   r0   rV   r*   r)   r!   r-   r,   rU   N)r7   )T)__name__
__module____qualname____firstlineno__pytestfixturer%   r>   rX   r]   r`   rg   rt   __static_attributes__r7   r'   r$   r   r      s    ^^L L2;4 ^^5 5 ^^6 6:. ^^L Lr'   r   c                      \ rS rSrS rS r\R                  R                  SS\	" S5      /5      \R                  R                  SS\
" S5      /5      S 5       5       rS rS	 r\R                  R                  S
\SS 0 \SS / \SS 4\SS SS0\SS / \SS 4\SS SS0/ \SS \SS 4\SS S/ SQ0\S   \S   /\S   /\S   \S   \S   /4/5      S 5       r\R                  R                  SSS/5      \R                  R                  S\S   \S   40 4\S   \S   40 4\S   4SS04\S   4SS04\S   \S   40 4\" S5      \S   40 4/5      S 5       5       r\R                  R                  S\SS 0 \SS / \SS 4\SS SS0\SS / \SS 4\SS SS0/ \SS \SS 4\SS S/ SQ0\S   \S   /\S   /\S   \S   \S   /4/5      S 5       r\R                  R                  S\SS 0 \SS 4/5      S 5       r\R                  R                  S\S   \S   40 4\S   \S   40 4\S   \S   40 4\" S5      \S   40 4/5      S 5       r\R                  R                  S S!S\S   40 4S"S\S   4SS04S#S\S   40 4S$S\S   4SS04S%S&\S   40 4S'S(\S   \S   -
  40 4S)S*\S   \S   -
  40 4S+S,\S   \S   -
  40 4S-S.\" S/5      40 4S0S1\" \
" S25      \	" S35      R4                  5      40 4S4S5\" \" S65      \	" S35      R4                  \	" S75      5      40 4/5      S8 5       r\R                  R                  S9S!S\" S5      \4S"S\" S5      \4S#S\" S5      \4S$S\" S5      \4S%S&\" S5      \4S'S(S:\4S)S*S:\4S+S,S:\4S-S.\" S5      \4S0S1\" S5      \4S4S5\" S5      \4/5      S; 5       r\R                  R                  S<S=0 \ " \SS> \SS> 5       V VVs/ s H	  u  pX-
  PM     snnn 4\S>   \S>   -
  \S   \S   -
  40 \ " \SS? \SS? 5       V VVs/ s H	  u  pX-
  PM     snnn 4/5      S@ 5       r!\R                  R                  S\S   \S   -
  \S>   \S>   -
  40 4\S   \S   -
  * \S>   \S>   -
  40 4\S   \S   -
  \S>   \S>   -
  /0 4/5      SA 5       r"\R                  R                  SBS=0 \S   \S   -
  \S   -   /4\S>   \S   -
  \S   \S   -   40 \S   \S   -
  \S   -   \S>   \S   -
  \S   \S   -   /4/5      SC 5       r#\R                  R                  S\S   \S   -
  \S   -   \S>   \S   -
  40 4\S   \S   -
  \S   -   * \S>   \S   -
  40 4\S   \S   -
  \S>   \S   -
  40 4/5      SD 5       r$\R                  R                  SBS=0 \S   \S   -
  \S   -   /4\S>   \S   -
  \S   \S   -   40 \S   \S   -
  \S   -   \S>   \S   -
  \S   \S   -   /4/5      SE 5       r%\R                  R                  S\S   \S   -
  \S   -   \S>   \S   -
  40 4\S   \S   -
  \S   -   * \S>   \S   -
  40 4\S   \S   -
  \S>   \S   -
  40 4\S   \S   -
  \S>   \S   -
  /0 4/5      SF 5       r&\R                  R                  SG/ / 4\S   \S   -
  \S   -   \S   \S   -
  \S   -   /4\S   \S   -   \S   \S   -
  /\S   \S   -   \S   \S   -
  /4/5      SH 5       r'SI r(SJ r)SK r*SL r+SM r,SN r-\R                  R                  SO/ SPQ5      SQ 5       r.\R                  R                  SR/ SSQ5      ST 5       r/\R                  R                  SU\0\1/5      SV 5       r2\R                  R                  SW0 \3" SXS/S\" SY5      //5      4SZS0\3" SS/S\" SY5      //5      4/5      S[ 5       r4\R                  R                  S\0 \3" SS/S\" SY5      //5      \3" \S   Rk                  \65      S/5      4/5      S] 5       r7\R                  R                  S^\0S_S0\4\1S_S0\4\0S-/ 0\4\0S`/ 0\4\1S-/ 0\4\1SaS0\4/5      Sb 5       r8Scr9gs  snnn f s  snnn f )d
TestSystemk   c                 X    U R                  5         U R                  R                  5         g N)r>   r!   validate_system)r#   r%   s     r$   test_empty_systemTestSystem.test_empty_systeml   s      "##%r'   c                 X    U R                  5         U R                  R                  5         g r   )rg   r!   r   r#   r]   s     r$   test_filled_systemTestSystem.test_filled_systemp   s    !!###%r'   r   Nr    c                    Uc  Uc  [        5       U l        O[        X5      U l        Uc'  U R                  R                  R                  S:X  d   eOU R                  R                  U:X  d   eUc'  U R                  R                  R                  S:X  d   eOU R                  R                  U:X  d   eU R                  5         [        U R                  R                  [        5      (       d   e[        U R                  R                  [        5      (       d   e[        U R                  R                  [        5      (       d   e[        U R                  R                  [        5      (       d   e[        U R                  R                  [        5      (       d   e[        U R                  R                  [        5      (       d   e[        U R                  R                  [        5      (       d   e[        U R                  R                  [        5      (       d   e[        U R                  R                   [        5      (       d   eg )Ninertial_pointinertial_frame)r   r!   r    namer   r>   
isinstancer)   r	   r*   r+   r,   r-   r.   r0   r1   r2   )r#   r   r    s      r$   	test_initTestSystem.test_initt   s    5= (DK 4DK;;**//3CCCC;;**k999=;;$$))-====;;$$---  "$++++_====$++++_====$++--9999$++++_====$++++_====$++--9999$++**O<<<<$++;;_MMMM$++>>PPPPr'   c                 *   [        S5      n[        R                  " U5      U l        U R                  R                  UR
                  :X  d   eU R                  R                  UR                  :X  d   eU R                  SS9  U4U R                  l        g )Nbody)r4   r:   )	r   r   from_newtonianr!   r    rq   r   r>   r4   )r#   rbs     r$   test_from_newtonian_rigid_body)TestSystem.test_from_newtonian_rigid_body   su    v++B/{{&&"--777{{  BHH,,,   5 Ur'   c                     [        S5      n[        R                  " [        5         [        R
                  " U5        S S S 5        g ! , (       d  f       g = f)Nparticle)r   rz   raises	TypeErrorr   r   )r#   pts     r$   test_from_newtonian_particle'TestSystem.test_from_newtonian_particle   s2    j!]]9%!!"% &%%s   A
Az)args, kwargs, exp_q_ind, exp_q_dep, exp_qrG   rH   TF)TFTr   rE   r   c                 ,   U R                   R                  " U0 UD6  U R                   R                  S S  U:X  d   eU R                   R                  S S  U:X  d   eU R                   R                  S S  U:X  d   eU R                  SS9  X@R                   l        XPR                   l        U R                   R                  S S  U:X  d   eU R                   R                  S S  U:X  d   eU R                   R                  S S  U:X  d   eU R                  SS9  g )N)r)   r*   r+   r   )r!   rL   r)   r*   r+   r>   )r#   r%   argskwargs	exp_q_ind	exp_q_depexp_qs          r$   test_coordinatesTestSystem.test_coordinates   s     	##T4V4{{  #y000{{  #y000{{}}Q5(((  )@ A%%{{  #y000{{  #y000{{}}Q5(((  )@ Ar'   funcrL   rM   zargs, kwargs   ac                     [         R                  " [        5         [        U R                  U5      " U0 UD6  S S S 5        U R                  5         g ! , (       d  f       N= fr   )rz   r   
ValueErrorr9   r!   rg   )r#   r]   r   r   r   s        r$   test_coordinates_speeds_invalid*TestSystem.test_coordinates_speeds_invalid   sB     ]]:&DKK&77 '!!# '&   A
Az)args, kwargs, exp_u_ind, exp_u_dep, exp_uc                 ,   U R                   R                  " U0 UD6  U R                   R                  S S  U:X  d   eU R                   R                  S S  U:X  d   eU R                   R                  S S  U:X  d   eU R                  SS9  X@R                   l        XPR                   l        U R                   R                  S S  U:X  d   eU R                   R                  S S  U:X  d   eU R                   R                  S S  U:X  d   eU R                  SS9  g )N)r,   r-   r.   r   )r!   rM   r,   r-   r.   r>   )r#   r%   r   r   	exp_u_ind	exp_u_depexp_us          r$   test_speedsTestSystem.test_speeds   s     	//{{  #y000{{  #y000{{}}Q5(((  )@ A%%{{  #y000{{  #y000{{}}Q5(((  )@ Ar'   zargs, kwargs, exp_u_auxc                    U R                   R                  " U0 UD6  U R                   R                  S S  U:X  d   eU R                  SS9  X@R                   l        U R                   R                  S S  U:X  d   eU R                  SS9  g )N)r/   r   )r!   rO   r/   r>   )r#   r%   r   r   	exp_u_auxs        r$   test_auxiliary_speeds TestSystem.test_auxiliary_speeds   s     	(($9&9{{  #y000   4%{{  #y000   4r'   c                     [         R                  " [        5         U R                  R                  " U0 UD6  S S S 5        U R                  5         g ! , (       d  f       N= fr   )rz   r   r   r!   rO   rg   r#   r]   r   r   s       r$   test_auxiliary_invalid!TestSystem.test_auxiliary_invalid   sA     ]]:&KK,,d=f= '!!# '&r   zprop, add_func, args, kwargsr)   r*   r,   r-   r/   rO   r0   rN   r1   rQ   r2   rR   r4   rp   r   r3   	add_loadsPNr6   add_actuatorsTAc                 Z   [        U R                  US5        USS4nUS;   a  US-  nU R                  US9  [        [	        U R                  U5      S S  5      / :X  d   e[	        U R                  U5      " U0 UD6  [        [	        U R                  U5      S S  5      [        U5      :X  d   eg )Nr7   r+   r.   )r1   r2   rT   r   )setattrr!   rg   listr9   )r#   r]   propadd_funcr   r   r:   s          r$   test_add_after_resetTestSystem.test_add_after_reset   s    $ 	T2&c"HH00G!!'!2GDKK.q12b888X&77GDKK.q12d4j@@@r'   zprop, add_func, value, error   c                 H   [         R                  " U5         [        U R                  U5      " U5        S S S 5        [         R                  " U5         [	        U R                  X$5        S S S 5        U R                  5         g ! , (       d  f       NT= f! , (       d  f       N0= fr   )rz   r   r9   r!   r   rg   )r#   r]   r   r   valueerrors         r$   test_type_errorTestSystem.test_type_error  sc      ]]5!DKK*51 "]]5!DKK- "!!#	 "!!!s   BB
B
B!zargs, kwargs, exp_kdesr7   rI      c                    U R                   R                  " U0 UD6  U R                  SS9  U R                   R                  S S  U:X  d   eX@R                   l        U R                  SS9  U R                   R                  S S  U:X  d   eg )N)r0   r   )r!   rN   rg   r0   )r#   r]   r   r   exp_kdess        r$   	test_kdesTestSystem.test_kdes"  s     	d-f-!!)!4{{"h...#!!)!4{{"h...r'   c                     [         R                  " [        5         U R                  R                  " U0 UD6  S S S 5        U R                  5         g ! , (       d  f       N= fr   )rz   r   r   r!   rN   rg   r   s       r$   test_kdes_invalidTestSystem.test_kdes_invalid1  sA     ]]:&KK  $1&1 '!!# '&r   zargs, kwargs, exp_conc                    SnU Vs/ s H  ofR                  [        5      PM     snU R                  -   nU R                  R                  " U0 UD6  U R                  US9  U R                  R                  S S  U:X  d   eU R                  R                  S S  U:X  d   eX@R                  l        U R                  US9  U R                  R                  S S  U:X  d   eU R                  R                  S S  U:X  d   eg s  snf )N)r1   rT   r   )rn   ro   rV   r!   rQ   rg   r1   rT   )r#   r]   r   r   exp_conr:   cexp_vel_cons           r$   test_holonomic_constraints%TestSystem.test_holonomic_constraints;  s     D*12'Qvvay'2TXX=--t>v>!!'!2{{003w>>>{{//2kAAA,3)!!'!2{{003w>>>{{//2kAAA 3s   C<c                     [         R                  " [        5         U R                  R                  " U0 UD6  S S S 5        U R                  5         g ! , (       d  f       N= fr   )rz   r   r   r!   rQ   rg   r   s       r$   "test_holonomic_constraints_invalid-TestSystem.test_holonomic_constraints_invalidO  sA     ]]:&KK114B6B '!!# '&r   c                    SnU R                   S [        U R                  5       U-   nU R                  R                  " U0 UD6  U R                  US9  U R                  R                  S S  U:X  d   eU R                  R                  S S  U:X  d   eX@R                  l        U R                  US9  U R                  R                  S S  U:X  d   eU R                  R                  S S  U:X  d   eg )N)r2   rT   r   )rU   lenrS   r!   rR   rg   r2   rT   )r#   r]   r   r   r   r:   r   s          r$   test_nonholonomic_constraints(TestSystem.test_nonholonomic_constraintsZ  s     Gggms477|,w600$A&A!!'!2{{33A6'AAA{{//2kAAA/6,!!'!2{{33A6'AAA{{//2kAAAr'   c                     [         R                  " [        5         U R                  R                  " U0 UD6  S S S 5        U R                  5         g ! , (       d  f       N= fr   )rz   r   r   r!   rR   rg   r   s       r$   %test_nonholonomic_constraints_invalid0TestSystem.test_nonholonomic_constraints_invalidn  sA     ]]:&KK44dEfE '!!# '&r   zconstraints, expectedc                     X R                   l        U R                  SS9  U R                   R                  S S  U:X  d   eg )Nr   r   )r!   rT   rg   )r#   r]   constraintsexpecteds       r$   #test_velocity_constraints_overwrite.TestSystem.test_velocity_constraints_overwritez  s=     ,7(!!*C!D{{//2h>>>r'   c                 
   [         S   [         S   -
  U R                  l        U R                  SS9  U R                  R                  S S  [         S   [         S   -
  /:X  d   eS U R                  l        U R                  5         g )NrG   rE   r   r   )rJ   r!   rT   rg   r   s     r$   &test_velocity_constraints_back_to_auto1TestSystem.test_velocity_constraints_back_to_auto  so    +-a52a5=(!!*C!D{{//2r!ur!u}oEEE+/(!!#r'   c                   ^ ^^ [        S5      [        S5      p2[        S5      [        S5      snmT R                  R                  X$5        T R                  R                  / T R                  QUPUP7:X  d   eT R                  R                  T5        T R                  R                  / T R                  QUPUPTP7:X  d   e/ T R                  l        T R                  R                  S:X  d   eTT R                  l        T R                  R                  T4:X  d   e[        S5      m[        R                  " [        U U4S j5        [        R                  " [        UU 4S j5        [        R                  " [        5         X#UTT4T R                  l        S S S 5        T R                  R                  T4:X  d   eg ! , (       d  f       N,= f)	Nrb1rb2p1p2r7   symbc                  :   > T R                   R                  T5      $ r   r!   rp   )r#   r   s   r$   <lambda>(TestSystem.test_bodies.<locals>.<lambda>  s    )?)?)Er'   c                  :   > TR                   R                  T 5      $ r   r   )r   r#   s   r$   r   r     s    $++*@*@*Dr'   )
r   r   r!   rp   r4   r   rz   r   r   r   )r#   r]   r   r   r   r   r   s   `    @@r$   test_bodiesTestSystem.test_bodies  sb   U#Yu%5S$$Bs'{{!!%<t{{%<C%<%<<<<r"{{!!%@t{{%@C%@%@R%@@@@{{!!R'''{{!!bU***vi!EFj"DE]]9%"%BD!9DKK &{{!!bU*** &%s   F>>
Gc                   ^^ [        5       m[        S5      [        S5      smn[        STS9n[        S5      n[	        SU5      nTR                  [        UTR                  5      X1R                  4[        XAR                  5      5        TR                  TTR                  4X1R                  4X1R                  44:X  d   eXR                  4/Tl	        TR                  XR                  44:X  d   e[        R                  " [        UU4S j5        [        R                  " [        5         TTR                  4Tl	        S S S 5        TR                  XR                  44:X  d   eg ! , (       d  f       N-= f)Nr   r   r   )r   mc1r   c                  T   > TR                  T T R                  T R                  45      $ r   )r   rs   y)r   r!   s   r$   r   +TestSystem.test_add_loads.<locals>.<lambda>  s    &*:*:AqssACC=*Ir'   )r   r   r   r   r   r   r   rs   r
   r3   rz   r   r   r   )r#   r   r   r   r   r   r!   s        @@r$   test_add_loadsTestSystem.test_add_loads  s   c"N3$71Q'EldC QSS)C:uR~F||ACC3*sCCjAAAACCz||CC{***j"IJ]]9%qss8FL &||CC{*** &%s   ,E''
E5c                 d   [        5       n[        S5      [        S5      p2[        [        S5      UR                  U5      n[        [        S5      UR
                  X#5      nUR                  U5        UR                  U4:X  d   eUR                  S:X  d   eU4Ul        UR                  U4:X  d   eg )Nr   r   T1T2r7   )	r   r   r   r   rs   r   r   r6   r3   )r#   r!   r   r   act1act2s         r$   test_add_actuatorsTestSystem.test_add_actuators  s    c"N3$71gdmQSS!4gdmQSS!7T"D7***||r!!! 7D7***r'   c           	        ^^^ [        S5      u  pp4pVn[        S[        S9u  mpp[        STXU5      n[	        SXX&5      m[        SXX75      n[        SXXDR                  [        5      5      n[        5       mTR                  U5        TR                  U4:X  d   eTR                  TU4:X  d   eTR                  [        U/5      :X  d   eTR                  [        U/5      :X  d   eTR                  [        XQR                  [        5      -
  /5      :X  d   eTR                  U
5        TR!                  U5        TR#                  XsR                  [        5      -
  5        TR                  U5        TR                  X4:X  d   eTR                  TXU	4:X  d   eTR                  [        X/5      :X  d   eTR                  [        XW/5      :X  d   eTR                  [        XQR                  [        5      -
  XsR                  [        5      -
  /5      :X  d   eTR#                  XbR                  [        5      -
  * 5        TR                  T5        TR                  XT4:X  d   eTR                  TXU	4:X  d   eTR                  [        XU/5      :X  d   eTR                  [        XWU/5      :X  d   eTR                  [        XQR                  [        5      -
  XsR                  [        5      -
  XbR                  [        5      -
  * /5      :X  d   eTR                  U5        TR                  XTU4:X  d   eTR                  TXX4:X  d   eTR                  [        XX$/5      :X  d   eTR                  [        XWXdR                  [        5      /5      :X  d   eTR                  [        XQR                  [        5      -
  XsR                  [        5      -
  XbR                  [        5      -
  * /5      :X  d   eTR$                  S S  / :X  d   eTR&                  S S  / :X  d   e[(        R*                  " [,        UU4S j5        [(        R*                  " [.        UU4S	 j5        g )
Nz	q1:5 u1:4zrb1:6rA   rC   rD   rF   J_lagc                  &   > TR                  T 5      $ r   rK   )rD   r!   s   r$   r   ,TestSystem.test_add_joints.<locals>.<lambda>  s    &*;*;B*?r'   c                  &   > TR                  T 5      $ r   r  )r   r!   s   r$   r   r    s    ):):3)?r'   )r   r   r   r   r   rn   ro   r   rK   r5   r4   r)   r	   r,   r0   rp   rL   rN   r*   r-   rz   r   r   r   )r#   q1q2q3q4u1u2u3r   rb3rb4rb5rC   rF   r  rD   r   r!   s                  @@@r$   test_add_jointsTestSystem.test_add_joints  s   %3K%@"")'y"ASsdC"-D#B3dCb-#B
;"}}%%%}}c
***||t4444||t4444{{orGGAJ.?@@@@#r"WWQZ("}}(((}}c 4444||x8888||x8888{{o''!*_b771:o.0 0 	0 0"wwqz/*+"}},,,}}c 4444|||<<<<|||<<<<{{orGGAJWWQZ13ggaj/A/C D D 	D D% }}U 3333}}c 9999||/?@@@@||GGAJ/GHHHH{{orGGAJWWQZ13ggaj/A/C D D 	D D||A"$$$||A"$$$j"?@i!?@r'   c                 J   U R                   SS  U R                  l         U R                  R                   U R                   SS  :X  d   eU R                  SS9  SU R                  l        SU R                  l        U R                   U R                  l         U R                  5         g )Nr   )r5   r   r7   )r5   r!   rg   r)   r,   r   s     r$   test_joints_setterTestSystem.test_joints_setter  s    ![[_{{!!T[[_444!!+!6![[!!#r'   zname, joint_index))rC   r   )rD   r   not_existingNc                 t    U R                   R                  U5      nUc  Ub   eg X@R                  U   :X  d   eg r   )r!   	get_jointr5   )r#   r]   r   joint_indexjoints        r$   test_get_jointTestSystem.test_get_joint  s=     %%d+= =KK4444r'   zname, body_index))r   r   )r  rE   r   c                 t    U R                   R                  U5      nUc  Ub   eg X@R                  U   :X  d   eg r   )r!   get_bodyr4   )r#   r]   r   
body_indexr   s        r$   test_get_bodyTestSystem.test_get_body  s=     {{##D)<<;;z2222r'   r8   c                      " S SU5      nU R                   R                  US9  [        U R                   R                  U5      (       d   eg )Nc                       \ rS rSrSrg):TestSystem.test_form_eoms_calls_subclass.<locals>.MyMethodi  r7   N)rv   rw   rx   ry   r|   r7   r'   r$   MyMethodr/    s    r'   r0  )r8   )r!   	form_eomsr   r8   )r#   rt   r8   r0  s       r$   test_form_eoms_calls_subclass(TestSystem.test_form_eoms_calls_subclass  sB    	z 	 	2$++00(;;;;r'   zkwargs, expectedrk   explicit_kinematicsc                 t    U R                   R                  " S0 UD6  U R                   R                  U:X  d   eg )Nr7   )r!   r1  mass_matrix_full)r#   rt   r   r   s       r$   !test_system_kane_form_eoms_kwargs,TestSystem.test_system_kane_form_eoms_kwargs  s1     	''{{++x777r'   zkwargs, mm, gmc                     U R                   R                  " SS[        0UD6  U R                   R                  U:X  d   eU R                   R                  U:X  d   eg )Nr8   r7   )r!   r1  r   r7  forcing_full)r#   rt   r   mmgms        r$   %test_system_lagrange_form_eoms_kwargs0TestSystem.test_system_lagrange_form_eoms_kwargs  sN     	CCFC{{++r111{{''2---r'   zeom_method, kwargs, errornon_existing_kwargkd_eqs
Lagrangianc                    [         S   U R                  l        [        S[	        S5      S9nU R                  R                  U5        UR                  R                  U R                  R                  [         S   U R                  R                  -  5        [        R                  " U5         U R                  R                  " SSU0UD6  S S S 5        g ! , (       d  f       g = f)Nr   rj   rk   rl   r8   r7   )r+   r!   r)   r   r   rp   rq   rr   r    rs   rz   r   r1  )r#   r%   r8   r   r   rj   s         r$   test_form_eoms_kwargs_errors'TestSystem.test_form_eoms_kwargs_errors!  s     aDSws|,q!	T[[44adT[[]]6JK]]5!KK!!BZB6B "!!s   +C
C!)r!   ):rv   rw   rx   ry   r   r   rz   markparametrizer   r   r   r   r   r+   r   r.   r   r   r   rP   r   r   rJ   r   r
   rs   r   r   r   r   r   rc   r   r   r   r   r   r   r   r   r   r  r	  r  r  r&  r+  r   r   r2  r	   r8  rn   ro   r>  rD  r|   ).0re   rf   s   000r$   r~   r~   k   s   && [[Wt^G-D&EF[[]T53G,HIQ J GQ0#&
 [[H	
2AAbqE2q!u%	
2A%q!ub!BQ%8	
2A&AbqE1Ra59	
2A 34qtQqTlQqTF
A$!ad			K BB  [[V&7%FG[[^
A$!r
A$!r
A$=%()
A$=%()
A$!r
#,!	r". $ H$ [[H	
2AAbqE2q!u%	
2A%q!ub!BQ%8	
2A&AbqE1Ra59	
2A 34qtQqTlQqTF
A$!ad			K BB  [[6	BQRV9 	5	5 [[^
Q%1
Q%1
Q%A
#,1	#	. $$
 [[;	#adWb1	#adW}e.DE	,1,	,1-)?@	(2a5(B7	beadl_b1	 "=
A$1+		#%C
A$1+		<)F"3!5r:	+eCj.2E2G2G HJBO	oCL.-//1D)F (HIK	M>  	A! 	A [[;	#WS\:>	#WS\:>	,j9	,j9	('#,
C	Q	*	 "=q)L	#%CQ			<y9	+ws|Y7	ows|Y?> $$ [[5	R#aeRV*<=*<wr"(*<=>
A$A,!r!u	%r"%aeRV"4	5"4wr"("4	5	78 
/
/ [[^
A$A,!r!u	%r*aD2a5L/1Q4"Q%<	("-Q4!A$;!r!u
%+. 
$
$
 [[4	R!A$1+!$%&
A$1+qtad{	#R
A$1+!
adQqTk1Q4!A$;	7	97 
B
B [[^
A$1+!
adQqTk	*B/aD1Q4K!A$
1!	-r2
A$1+qtad{	#R(. 
$
$ [[4	R!A$A,1%&'
A$1+qtad{	#R
A$A,1
qtad{AaD1Q4K	8	:7 
B
B [[^
A$A,1
qtad{	+R0aD2a5L1Q4
 !A$1+	.3
A$1+qtad{	#R(Q4!A$;!qt
$r*	. $$ [[4	R	AAA	AAA!6 78
Q%"Q%-1!	%11qtad{'CD7 
?
?$+$+	+*AX$ [[0 3 
5
5 [[/ 2 
3
3 [[\K+IJ< K< [[/	_r1g73<'89:;
	&!Q:;WS\9J9L )M 	N2 
8
8
 [[-	_q!fq'#,&789	!A$))A,*	+	-0 .	. [[8	+Q/;	/3Y?	xnj1	xnj1	8R.*5	<+Z8; CCM >	5s   "a% a,r~   c                   8   \ rS rSr\R
                  R                  S\\S4\\S4/5      S 5       r	\R
                  R                  S\S4\S4/5      S 5       r
S rS	 rS
 r\R
                  R                  S\S4\S4/5      S 5       rS rS rS rS rS rSrg)TestValidateSystemi3  z)valid_method, invalid_method, with_speedsTFc                     U R                  US9  U R                  R                  U5        [        R                  " [
        5         U R                  R                  U5        S S S 5        g ! , (       d  f       g = f)Nr[   )rX   r!   r   rz   r   r   )r#   valid_methodinvalid_methodrW   s       r$   test_only_valid"TestValidateSystem.test_only_valid4  sQ    
 	""{";##L1]]:&KK''7 '&&s   A**
A8zmethod, with_speedsc                   ^ ^ T R                  US9  T R                  SS  T R                  l        T R                  S S T R                  l        T R                  S S T R                  l        [
        R                  " [        UU 4S j5        g )Nr[   r   r4  c                  :   > TR                   R                  T 5      $ r   r!   r   methodr#   s   r$   r   BTestValidateSystem.test_missing_joint_coordinate.<locals>.<lambda>F      $++*E*Ef*Mr'   )rX   r)   r!   r,   r0   rz   r   r   r#   rT  rW   s   `` r$   test_missing_joint_coordinate0TestValidateSystem.test_missing_joint_coordinate?  sg     	""{"; JJqrN JJsO99Sb>j"MNr'   c                    ^  T R                   S S T R                  l         T R                  SS  T R                  l        T R                  S S T R                  l        [        R
                  " [        U 4S j5        g )Nr4  r   c                  8   > T R                   R                  5       $ r   rR  r"   s   r$   r   =TestValidateSystem.test_missing_joint_speed.<locals>.<lambda>L      $++*E*E*Gr'   )r)   r!   r,   r0   rz   r   r   r   s   ` r$   test_missing_joint_speed+TestValidateSystem.test_missing_joint_speedH  sT     JJsO JJqrN99Sb>j"GHr'   c                    ^  T R                   SS  T R                  l         [        R                  " [        U 4S j5        g )Nr   c                  8   > T R                   R                  5       $ r   rR  r"   s   r$   r   <TestValidateSystem.test_missing_joint_kdes.<locals>.<lambda>P  r]  r'   )r0   r!   rz   r   r   r   s   ` r$   test_missing_joint_kdes*TestValidateSystem.test_missing_joint_kdesN  s(    99QR=j"GHr'   c                     U R                   S   * /U R                   SS  -   U R                  l         U R                  R                  5         g )Nr   r   )r0   r!   r   r   s     r$   test_negative_joint_kdes+TestValidateSystem.test_negative_joint_kdesR  s9    !YYq\M?TYYqr]:##%r'   c                   ^ ^ T R                  US9  / T R                  l        T R                  T R                  S   T R
                  S   -
  T R                  S   -   /-   T R                  l        [        R                  " [        UU 4S j5        / T R                  l
        T R                  T R                  -   T R                  l        T R                  R                  T5        g )Nr[   r   r   c                  :   > TR                   R                  T 5      $ r   rR  rS  s   r$   r   FTestValidateSystem.test_missing_holonomic_constraint.<locals>.<lambda>]  rV  r'   )rX   r!   r1   rV   r,   r-   r2   rz   r   r   r*   r)   r   rW  s   `` r$   !test_missing_holonomic_constraint4TestValidateSystem.test_missing_holonomic_constraintV  s     	""{";,.)/3xxJJqMDJJqM)DJJqM9;; 0;,j"MN JJ3##F+r'   c                 .  ^  / T R                   l        [        R                  " [        U 4S j5        T R
                  S   T R                   l        T R                  T R
                  S   /-   T R                   l        T R                   R                  5         g )Nc                  8   > T R                   R                  5       $ r   rR  r"   s   r$   r   ITestValidateSystem.test_missing_nonholonomic_constraint.<locals>.<lambda>d  r]  r'   r   r   )r!   r2   rz   r   r   r-   r,   r   r   s   ` r$   $test_missing_nonholonomic_constraint7TestValidateSystem.test_missing_nonholonomic_constraintb  sa    /1,j"GH JJqM JJ$**Q-8##%r'   c                   ^  T R                   [        S   /-   T R                  l         T R                  [        S   [        S   -
  /-   T R                  l        T R                  R                  5         T R                  T R                  l        T R                   S S T R                  l         T R                  S S T R                  l        [        R                  " [        U 4S j5        g )Nr   r4  c                  8   > T R                   R                  5       $ r   rR  r"   s   r$   r   FTestValidateSystem.test_number_of_coordinates_speeds.<locals>.<lambda>r  r]  r'   )
r,   r.   r!   r0   rJ   r   r)   rz   r   r   r   s   ` r$   !test_number_of_coordinates_speeds4TestValidateSystem.test_number_of_coordinates_speedsi  s     JJ!A$/99!r!u~5##% JJ JJsO99Sb>j"GHr'   c                 2  ^  T R                   S S T R                  l         [        R                  " [        U 4S j5        T R                   [
        S   [
        S   -   [        S   -
  /-   T R                  l         [        R                  " [        U 4S j5        g )Nr4  c                  8   > T R                   R                  5       $ r   rR  r"   s   r$   r   8TestValidateSystem.test_number_of_kdes.<locals>.<lambda>w  r]  r'   rE   r   c                  8   > T R                   R                  5       $ r   rR  r"   s   r$   r   ry  y  r]  r'   )r0   r!   rz   r   r   r.   rJ   r   s   ` r$   test_number_of_kdes&TestValidateSystem.test_number_of_kdest  sd    99Sb>j"GH99!qtbe(;'<<j"GHr'   c                 6    U R                   R                  SS9  g )NT)check_duplicatesrR  r   s     r$   test_duplicates"TestValidateSystem.test_duplicates{  s    ##T#:r'   c           	         [         S [        U R                  5       U R                  l        [        R
                  " [        5         U R                  R                  [        5        S S S 5        / U R                  l        U R                  R                  [        5        [        U R                  l
        [        R
                  " [        5         U R                  R                  [        5        S S S 5        / U R                  l
        U R                  R                  [        5        U R                  R                  [        S[        S5      [        S5      5      5        / U R                  l        [        R
                  " [        5         U R                  R                  [        5        S S S 5        g ! , (       d  f       GNR= f! , (       d  f       N= f! , (       d  f       g = f)NJurbu1rbu2)r.   r   r,   r!   rz   r   r   r   r   rP   r/   rK   r   r   )r#   r`   s     r$   test_speeds_in_lagrange*TestValidateSystem.test_speeds_in_lagrange  s"   .s4::/]]:&KK''8 '##O4]]:&KK''8 '##O4T9V,i.?@	B]]:&KK''8 '& '&
 '& '&s$    F1 G G1
G 
G
G"r7   N)rv   rw   rx   ry   rz   rF  rG  r   r   rN  rX  r^  rc  rf  rk  rp  ru  r{  r  r  r|   r7   r'   r$   rJ  rJ  3  s    [[H	ot,	+u-K 8	8 [[2	dou557 8O8OII& [[2	dou557 8,8,&	II;9r'   rJ  c                   &    \ rS rSrS rS rS rSrg)TestSystemExamplesi  c                 .   [        S5      u  pp4n[        S5      u  pgpn
[        S5      n[        SUS9n[        SUS9n[	        S5      n[
        R                  " U5      nUR                  U4:X  d   eUR                  UR                  :X  d   eUR                  UR                  :X  d   e[        SXXUR                  S	9n[        S
XXyUR                  XUR                  -  S9nUR!                  UU5        UR"                  UU4:X  d   eUR%                  S5      U:X  d   eUR'                  S5      U:X  d   eUR)                  U* UR                  -  5        UR+                  UR                  XkR                  -  45        UR-                  [/        XW-  UR                  X5      5        UR1                  5         UR3                  5         [5        UR6                  [8        5      (       d   e[;        UR<                  [?        XC-   XB-  [A        U5      -  /XB-  [A        U5      -  XBS-  -  //5      -
  5      [C        SS5      :X  d   e[;        URD                  [?        XB-  U	S-  -  [G        U5      -  U-   /U* U-  U-  [G        U5      -  XW-  -   //5      -
  5      [C        SS5      :X  d   eURI                  [K        UR                  RM                  UR                  5      RO                  UR                  5      5      5        UR6                  b   eXxsUl(        Ul)        XsUl*        Ul+        UR1                  5         X* [G        U5      -  X* [A        U5      -  U	-  U
RY                  [Z        5      X)S-  [G        U5      -  U	RY                  [Z        5      [A        U5      -  -
  -  0nU* U-  [G        U5      -  XW-  U-  -   X#-  [G        SU-  5      -  U	S-  -  S-  -   X$-  [G        SU-  5      -  U	S-  -  S-  -
  U[A        U5      -  -
  X#[A        U5      S-  -  U[G        U5      S-  -  -   -  -  n[]        [_        UR3                  5       Ra                  U5      U	RY                  [Z        5      5      Rc                  5       5      S   n[;        UU-
  5      S:X  d   e[5        UR6                  [8        5      (       d   e[?        SS/SS//5      * n[?        X/5      * n[?        US-  * U-  [A        U5      S-  -  US-  U-  -   X$-  [A        U5      -  X#U-   -  [A        U5      -  -
  /U[A        U5      -  S//5      n[?        U* U-  U-  [G        U5      -  XW-  -   US-  U-  U	S-  -  [G        U5      -  [A        U5      -  -
  X&-  [A        U5      -  -
  /X)S-  -  [G        U5      -  //5      nURe                  [C        SS5      5      Rg                  [C        SS5      Re                  U5      5      nURg                  U5      n[;        UR<                  U-
  5      [C        SS5      :X  d   e[;        URD                  U-
  5      [C        SS5      :X  d   e[;        URh                  U-
  5      [C        SS5      :X  d   e[;        URj                  U-
  5      [C        SS5      :X  d   eg )Ng l mc mp kzF qp qc up ucrailcartrl   bob	bob_frameslider
joint_axispinr  child_interframechild_pointrE   r   r   rI   )6r   r   r   r   r   r   r   r4   r   r    rq   r   rs   r   zr   rK   r5   r#  r)  apply_uniform_gravityr   r   r   r   r1  r   r8   r   r   mass_matrixr	   r   r   forcingr   rQ   r   pos_fromdotr)   r*   r,   r-   rn   ro   rd   r   xreplacevaluesrow_joincol_joinr7  r;  )r#   glmcmpkFqpqcupucr  r  r  r  r!   r  r  subsupd_expectedupd_solMkgkMdgdMmr=  s                              r$   test_cart_pendulum_kanes+TestSystemExamples.test_cart_pendulum_kanes  sk    "-0ba*?;rr b)u2&";/	&&t,}}'''||tzz)))!!T__444$bPudDFF(19;;P&#&}}---)V333u%,,,$$aR&((]3$//1vv:67^AFDFFILM &++[9999++ogrvB'(26CG+;Rq&[*IJ/L L MA; 	   /VbAgB'!+,S1Wq[3r7"QV+,;. +/ / 038A;? 	@ ? 	((CNN++DOO<@@JK	M  (((%'"fl%'"fl  BRLBRL2%
Aq3r7!2RWWQZ#b'5I!IJL R"Ws2w!+afs1r6{.BR1W.Lq.PPvAF#bAg-1245BK@s2w!|#b3r7a<&778: 	 f..099$? ggaj**0&(4457,./1444&++[9999 1v1v.//rh''Qw|c"gl:Q!Vb[H vB/!Bw-#b'2IIK 3r7{A.0 1 b1frkCG#af,qAv{R1W/Ds2w/N"g0 B( )+,Qw;R+@*ACD kk%1+&00q!1E1Eb1IJ[[_**R/0E!QK???+,a;;;//"45q!DDD++b01U1a[@@@r'   c                    [        S5      u  pp4n[        S5      u  pgn[        SS5      u  p[        S5      n[        SUS9n[        SUS9n[	        S	5      n[
        R                  " U5      nUR                  U4:X  d   eUR                  UR                  :X  d   eUR                  UR                  :X  d   e[        S
XXUR                  S9n[        SXXyUR                  XUR                  -  S9nUR!                  UU5        UR"                  UU4:X  d   eUR%                  S
5      U:X  d   eUR'                  S5      U:X  d   eUR                   HV  nUR(                  U-  UR                  R+                  UR                  5      R-                  UR                  5      -  Ul        MX     UR1                  UR                  XkR                  -  45        UR3                  [5        XW-  UR                  X5      5        UR7                  [8        5        UR;                  [8        5        [=        UR>                  [A        XC-   XB-  [C        U5      -  /XB-  [C        U5      -  XBS-  -  //5      -
  5      [E        SS5      :X  d   e[=        URF                  [A        XB-  U	S-  -  [I        U5      -  U-   /U* U-  U-  [I        U5      -  XW-  -   //5      -
  5      [E        SS5      :X  d   eURK                  [M        UR                  R+                  UR                  5      R-                  UR                  5      5      5        URN                  b   eXxsUl(        Ul)        X* [I        U5      -  X* [C        U5      -  U	-  U
RU                  [V        5      X)S-  [I        U5      -  U	RU                  [V        5      [C        U5      -  -
  -  0nU* U-  [I        U5      -  XW-  U-  -   X#-  [I        SU-  5      -  U	S-  -  S-  -   X$-  [I        SU-  5      -  U	S-  -  S-  -
  U[C        U5      -  -
  X#[C        U5      S-  -  U[I        U5      S-  -  -   -  -  nUR;                  [8        5      nURN                  RX                  S   nURN                  R[                  5       U   n[]        US   R_                  UU05      R_                  U5      U	RU                  [V        5      5      S   n[=        UU-
  5      S:X  d   e[a        URN                  [8        5      (       d   e[A        US-  U-  X$-  [C        U5      -  U* [C        U5      -  /X$-  [C        U5      -  X4-   S//5      n[A        U* U-  U-  [I        U5      -  XW-  -   /X$-  [I        U5      -  U	S-  -  U-   //5      n[c        S5      Re                  [E        SS5      5      Rg                  [E        SS5      Re                  URg                  [A        U[C        U5      -  SS/5      Rh                  5      5      5      n[A        X/US S  -   U[I        U5      -  U	S-  -  /-   5      n[=        UR>                  U-
  5      [E        SS5      :X  d   e[=        URF                  U-
  5      [E        SS5      :X  d   e[=        URj                  U-
  5      [E        SS5      :X  d   e[=        URl                  U-
  5      [E        SS5      :X  d   eg )Nr  zF qp qczqp qcr   r  r  rl   r  r  r  r  r  r  rE   r   r4  rG   r   )7r   r   r   r   r   r   r   r4   r   r    rq   r   rs   r   r  r   rK   r5   r#  r)  rm   r  r  potential_energyr   r   r   r   r   r1  r   r  r	   r   r   r  r   rQ   r   r8   r)   r*   rn   ro   lam_vecsolve_multipliersr   r  r   r   r  r  r   r7  r;  )r#   r  r  r  r  r  r  r  r  qpdqcdr  r  r  r  r!   r  r  r   r  qpdd_expectedeomslam1lam1_solqpdd_solr  r  r  r=  s                                r$   test_cart_pendulum_lagrange.TestSystemExamples.test_cart_pendulum_lagrange  s    "-0ba"9-	r!'1- b)u2&";/	&&t,}}'''||tzz)))!!T__444$b+/663udTVV(19;;P&#&}}---)V333u%,,,MMD$(IIMDOO4L4L""5$$'CM%2D! " 	$//1vv:67^AFDFFILM/)++ogrvB'(26CG+;Rq&[*IJ/L L MA; 	   /VcQhR(1,-a!c"g0E0N/O;Q +
 
 Aqk 	  	((CNN++DOO<@@JK	M  (((%'"fl BRLR#b'\C'Q(SW"4sxx{SW7L"LMO R"Ws2w!+afs1r6{.BSAX.M/ #a"f+%q0145783r7{Cs2w!|#b3r7a<&778: 	 0  ((+$$668>a))4*:;DDTJ!&&')=01Q666&++_==== qAv{AFSW,<qb3r7lK vB/"=? @b1frkCG#af,-fs2w)A-.01 !fooeAqk*44U1a[5I5IKKSWa(;<>>?6A BcZ"Q%/1s2w;3I2JJK**R/0E!QK???+,a;;;//"45q!DDD++b01U1a[@@@r'   c           	         [        S5      u  pn[        S5      u  pEn[        SSS9u  px[        SUS9n	[        5       n
U
R	                  U	5        U	R
                  R                  U
R                  XJR                  -  5        U	R
                  R                  U
R                  XZR                  -  XjR                  -  -   5        U/U/U/sU
l        U
l        U
l        UR                  [         5      U-
  /U
l        U
R%                  U* U
R                  -  5        U
R'                  [)        XU
R                  -  5      [)        XU
R                  -  X7-  U
R                  -  -
  5      5        U
R+                  5         U
R-                  5         [/        S/5      n[/        U/5      n[/        U/5      n[/        XU-  -
  /5      nUR1                  [3        SS5      5      R5                  [3        SS5      R1                  U5      5      nUR5                  U5      n[/        XrU-  -
  /5      n[7        U
R8                  U-
  5      [3        SS5      :X  d   e[7        U
R:                  U-
  5      [3        SS5      :X  d   e[7        U
R<                  U-
  5      [3        S	S	5      :X  d   e[7        U
R>                  U-
  5      [3        S	S5      :X  d   e[7        U
R@                  RB                  U-
  5      [3        SS5      :X  d   eg )
Nzg m muzq u uazN FT)positiver   rl   r   rE   )"r   r   r   r   rp   rq   rr   r    rs   set_velr   r   r)   r,   r/   rn   ro   r0   r  r   r
   r   r1  r	   r  r   r  r   r  r  r7  r;  r8   auxiliary_eqs)r#   r  rk   mur+   r.   rP   r   r  r   r!   r  r  r  r  r  r=  aux_eqss                     r$   test_box_on_ground%TestSystemExamples.test_box_on_ground  s    8$b!(+bed3Sq!!	V//XX>	V\\1xx<"xx-+GH453bT0flFLvvay1}o$$aR&((]3!\"!\BFVXX$556	8 	  aS!aS!aS!aq&j\*kk%1+&00q!1E1Eb1IJ[[_!11u9+.**R/0E!QK???+,a;;;//"45q!DDD++b01U1a[@@@))77'A "1a[) 	) )r'   r7   N)rv   rw   rx   ry   r  r  r  r|   r7   r'   r$   r  r    s    CAJEAN!)r'   r  ))rz   sympy.core.symbolr   sympy.core.sympifyr   (sympy.functions.elementary.trigonometricr   r   sympy.matrices.denser   r   sympy.matrices.immutabler	   sympy.physics.mechanicsr
   r   r   r   r   r   r   r   r   r   r   r   r   sympy.simplify.simplifyr   sympy.solvers.solversr   _tro   r+   rJ   r.   rP   r   r~   rJ  r  r7   r'   r$   <module>r     s     % & = + 4    - '5E15FRL RLjEC ECP[9 [9|n) n)r'   