
    QiK                     z   d dl Z d dlmZ d dlmZ d dlmZmZ d dlm	Z	m
Z
 d dlmZ d dlmZmZmZmZmZmZmZmZmZmZmZmZmZ d dlmZ d d	lmZ ej        Z  ed
          Z! ed
d          Z" ed          Z# ed          Z$ G d d          Z% G d de%          Z& G d de%          Z' G d d          Z(dS )    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                       e Zd Z ej                    d             ZddZddZ ej                    d             Z ej                    d             Z	ddZ
 ej                    d	             Zd
S )TestSystemBasec                 d    t          t          d          t          d                    | _        d S )Nframefixed_point)r   r   r   systemselfs    /var/www/development/aibuddy-work/election-extract/venv/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_system_class.py_empty_system_setupz"TestSystemBase._empty_system_setup   s'    ^G44eM6J6JKK     c                     d}d}|D ])}||vr#t          | j        |          d d          g k    sJ *|D ]!}||vrt          | j        |          dk    sJ "d|vr| j        j        J d S d S )N)
q_indq_depqu_indu_depuu_auxkdesholonomic_constraintsnonholonomic_constraints)loadsbodiesjoints	actuatorsr&   
eom_method)getattrr    r6   )r"   excludematricestuplesattrs        r#   _empty_system_checkz"TestSystemBase._empty_system_check   s    Q; 	; 	;D7""t{D11!!!4:::: 	8 	8D7""t{D11R7777w&&;)111 '&11r%   Tc           
         t          t          d          t          d                    | _        |rt	          d          nt
          }t          dt                    | _        t          d| j        d         | j        d         t          d         |d                   t          d	| j        d         | j        d
         t          d         |d                   t          d| j        d
         | j        d         t          d
         |d
                   f| _         | j        j        | j          | j                            t          d         dg           | j                            |d         d           |r_| j                            |d         t
          d         z
             | j                            t$          d         t$          d                    | j                            t          d
         t          d         z
  t          d         z              | j                            |d         t
          d         z
  |d
         z              |d d
         | j        _        |d
d         | j        _        | j        j        d d          | j        j        d d          c| _        | _        | j        j        d d          | j        j        d d          c| _        | _        | j        j        d d          | _        | j        j        d d          | _        | j        j        d d          | _        | j        j        d d          | _        d S )Nr   r   r   zrb1:5clsJ1r   r   J2   J3   F)independent   ) r   r   r   r    r   qdr   r   r3   r   r*   r   r4   
add_jointsadd_coordinates
add_speedsadd_kdesadd_auxiliary_speedsuaadd_holonomic_constraintsadd_nonholonomic_constraintsr+   r,   r(   r)   r/   r0   hcvelocity_constraintsvcr1   nhc)r"   with_speedsr-   s      r#   _create_filled_systemz$TestSystemBase._create_filled_system(   s   ^G44eM6J6JKK%08N5!!!bg9555T4;q>4;q>1Q41FF4QQ1qtLLT4;q>4;q>1Q41FF

 	,,##AaDug#>>>qt777 	;K  11...K,,RUBqE:::--adQqTkAaD.@AAA0011!1DEEEbqbEacF!%!2111!5t{7H7K
DJ!%!2111!5t{7H7K
DJK$QQQ'	+3AAA6+21115;7:r%   c                 2    |                      d           d S )NTrT   rU   r!   s    r#   _filled_system_setupz#TestSystemBase._filled_system_setupB   s    ""t"44444r%   c                 2    |                      d           d S )NFrW   rX   r!   s    r#   _filled_system_setup_no_speedsz-TestSystemBase._filled_system_setup_no_speedsF   s    ""u"55555r%   c                    d|v s'| j         j        d d          t          d d         k    sJ d|v s&| j         j        d d          t          d         gk    sJ d|v s'| j         j        d d          t          d d         k    sJ d|v s'| j         j        d d          t
          d d         k    sJ d|v s'| j         j        d d          t
          dd         k    sJ d	|v s'| j         j        d d          t
          d d         k    sJ d
|v s'| j         j        d d          t          d d         k    sJ d|v sL| j         j	        d d          d t          t
          d d         t          d d                   D             k    sJ d|v sB| j         j        d d          t          d         t          d         z
  t          d         z   gk    sJ d|v sB| j         j        d d          t
          d         t          d         z
  t
          d         z   gk    sJ d|v sj| j         j        d d          t          d         t          d         z
  t          d         z   t
          d         t          d         z
  t
          d         z   gk    sJ d|v s$| j         j        t!          | j                  k    sJ d|v s$| j         j        t!          | j                  k    sJ d S d S )Nr(   rD   r)   r*   rF   r+   rB   r,   r-   r.   r/   c                     g | ]
\  }}||z
  S r&   r&   .0uiqdis      r#   
<listcomp>z7TestSystemBase._filled_system_check.<locals>.<listcomp>R   s-     <8 <8 <8 SBH<8 <8 <8r%   r0   r   r   r1   rQ   r3   r4   )r    r(   r*   r)   r+   r-   r,   r.   rM   r/   ziprG   r0   r1   rQ   r3   tupler4   )r"   r8   s     r#   _filled_system_checkz#TestSystemBase._filled_system_checkJ   s   '!!T[%6qqq%9QrrU%B%B%B%B'!!T[%6qqq%9adV%C%C%C%Cg~~qqq!1QrrU!:!:!:!:'!!T[%6qqq%9QrrU%B%B%B%B'!!T[%6qqq%9QqsV%C%C%C%Cg~~qqq!1QrrU!:!:!:!:'!!T[%6qqq%9RV%C%C%C%C  DK$4QQQ$7 <8 <8$'"1"r"1"v$6$6<8 <8 <8 %8 %8 %8 %8'7221!!!41!qt9K8LLLLL*g554QQQ7AaD2a5L1Q4<O;PPPPP&'1103qEBqEMBqE)1Q4"Q%<!A$+>8@ @ @ @ @G##"eDK&8&88888G##"eDK&8&88888 $#88r%   c                    t           d         | j        _        t          d         | j        _        t          d         t           d                             t                    z
  | j        _        t          dt          d                    }| j        
                    |           |j                            | j        j        t           d         | j        j        z             d S )Nr   pmmass)r*   r    r(   r-   r+   difftr/   r   r   
add_bodies
masscenterset_posr   x)r"   r$   rg   s      r#   _moving_point_massz!TestSystemBase._moving_point_massa   s    aDaDQ4!A$))A,,.Sws||,,,q!!!	T[4adT[]6JKKKKKr%   N)r&   )T)__name__
__module____qualname__pytestfixturer$   r<   rU   rY   r[   re   rq   r&   r%   r#   r   r      s        V^L L L2 2 2 2; ; ; ;4 V^5 5 5 V^6 6 6: : : :. V^L L L L Lr%   r   c                      e Zd Zd Zd Zej                            dd ed          g          ej                            dd e	d          g          d                         Z
d Zd Zej                            d	edd
         i edd
         g edd
         fedd
         ddiedd
         g edd
         fedd
         ddig edd
         edd
         fedd
         dg died         ed         ged         ged         ed         ed         gfg          d             Zej                            dddg          ej                            ded         ed         fi fed         ed         fi fed         fddifed         fddifed         ed         fi f ed          ed         fi fg          d                         Zej                            dedd
         i edd
         g edd
         fedd
         ddiedd
         g edd
         fedd
         ddig edd
         edd
         fedd
         dg died         ed         ged         ged         ed         ed         gfg          d             Zej                            dedd
         i edd
         fg          d             Zej                            ded         ed         fi fed         ed         fi fed         ed         fi f ed          ed         fi fg          d             Zej                            dd ded         fi fd!ded
         fddifd"ded         fi fd#ded
         fddifd$d%ed         fi fd&d'ed         ed         z
  fi fd(d)ed         ed         z
  fi fd*d+ed         ed         z
  fi fd,d- ed.          fi fd/d0 e e	d1           ed2          j                  fi fd3d4 e ed5           ed2          j         ed6                    fi fg          d7             Zej                            d8d d ed          efd!d ed          efd"d ed          efd#d ed          efd$d% ed          efd&d'd9efd(d)d9efd*d+d9efd,d- ed          efd/d0 ed          efd3d4 ed          efg          d:             Zej                            d;d<i d=  eedd>         edd>                   D             fed>         ed>         z
  ed         ed         z
  fi d?  eedd@         edd@                   D             fg          dA             Z ej                            ded         ed         z
  ed>         ed>         z
  fi fed         ed         z
   ed>         ed>         z
  fi fed         ed         z
  ed>         ed>         z
  gi fg          dB             Z!ej                            dCd<i ed         ed         z
  ed         z   gfed>         ed         z
  ed         ed
         z   fi ed         ed         z
  ed         z   ed>         ed         z
  ed         ed
         z   gfg          dD             Z"ej                            ded         ed         z
  ed         z   ed>         ed
         z
  fi fed         ed         z
  ed         z    ed>         ed
         z
  fi fed         ed         z
  ed>         ed
         z
  fi fg          dE             Z#ej                            dCd<i ed
         ed         z
  ed         z   gfed>         ed         z
  ed         ed
         z   fi ed
         ed         z
  ed         z   ed>         ed         z
  ed         ed
         z   gfg          dF             Z$ej                            ded
         ed         z
  ed         z   ed>         ed
         z
  fi fed
         ed         z
  ed         z    ed>         ed
         z
  fi fed         ed         z
  ed>         ed
         z
  fi fed         ed         z
  ed>         ed
         z
  gi fg          dG             Z%ej                            dHg g fed         ed         z
  ed         z   ed         ed         z
  ed         z   gfed         ed         z   ed         ed         z
  ged         ed         z   ed         ed         z
  gfg          dI             Z&dJ Z'dK Z(dL Z)dM Z*dN Z+dO Z,ej                            dPg dQ          dR             Z-ej                            dSg dT          dU             Z.ej                            dVe/e0g          dW             Z1ej                            dXi  e2dYdgd edZ          gg          fd[di e2ddgd edZ          gg          fg          d\             Z3ej                            d]i  e2ddgd edZ          gg           e2ed         4                    e5          dg          fg          d^             Z6ej                            d_e/d`diefe0d`diefe/d,g iefe/dag iefe0d,g iefe0dbdiefg          dc             Z7dS )d
TestSystemc                 `    |                                   | j                                         d S N)r<   r    validate_system)r"   r$   s     r#   test_empty_systemzTestSystem.test_empty_systeml   s.      """##%%%%%r%   c                 `    |                                   | j                                         d S rz   )re   r    r{   r"   rY   s     r#   test_filled_systemzTestSystem.test_filled_systemp   s.    !!#####%%%%%r%   r   Nr   c                    ||t                      | _        nt          ||          | _        || j        j        j        dk    sJ n| j        j        |k    sJ || j        j        j        dk    sJ n| j        j        |k    sJ |                                  t          | j        j        t                    sJ t          | j        j	        t                    sJ t          | j        j
        t                    sJ t          | j        j        t                    sJ t          | j        j        t                    sJ t          | j        j        t                    sJ t          | j        j        t                    sJ t          | j        j        t                    sJ t          | j        j        t                    sJ d S )Ninertial_pointinertial_frame)r   r    r   namer   r<   
isinstancer(   r	   r)   r*   r+   r,   r-   r/   r0   r1   )r"   r   r   s      r#   	test_initzTestSystem.test_initt   s    5= ((DKK 44DK;*/3CCCCCC;*k9999=;$)-======;$----  """$++_=====$++_=====$+-99999$++_=====$++_=====$+-99999$+*O<<<<<$+;_MMMMM$+>PPPPPPPr%   c                     t          d          }t          j        |          | _        | j        j        |j        k    sJ | j        j        |j        k    sJ |                     d           |f| j        _        d S )Nbody)r3   r8   )	r   r   from_newtonianr    r   rn   r   r<   r3   )r"   rbs     r#   test_from_newtonian_rigid_bodyz)TestSystem.test_from_newtonian_rigid_body   sz    v+B//{&"-7777{ BH,,,,   555 Ur%   c                     t          d          }t          j        t                    5  t	          j        |           d d d            d S # 1 swxY w Y   d S )Nparticle)r   ru   raises	TypeErrorr   r   )r"   pts     r#   test_from_newtonian_particlez'TestSystem.test_from_newtonian_particle   s    j!!]9%% 	& 	&!"%%%	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	&s   AAAz)args, kwargs, exp_q_ind, exp_q_dep, exp_qrD   rE   TF)TFTr   rB   r   c                     | j         j        |i | | j         j        d d          |k    sJ | j         j        d d          |k    sJ | j         j        d d          |k    sJ |                     d           || j         _        || j         _        | j         j        d d          |k    sJ | j         j        d d          |k    sJ | j         j        d d          |k    sJ |                     d           d S )N)r(   r)   r*   r   )r    rI   r(   r)   r*   r<   )r"   r$   argskwargs	exp_q_ind	exp_q_depexp_qs          r#   test_coordinateszTestSystem.test_coordinates   s     	$#T4V444{ #y0000{ #y0000{}QQQ5((((  )@ AAA%%{ #y0000{ #y0000{}QQQ5((((  )@ AAAAAr%   funcrI   rJ   zargs, kwargs   ac                     t          j        t                    5   t          | j        |          |i | d d d            n# 1 swxY w Y   |                                  d S rz   )ru   r   
ValueErrorr7   r    re   )r"   rY   r   r   r   s        r#   test_coordinates_speeds_invalidz*TestSystem.test_coordinates_speeds_invalid   s     ]:&& 	8 	8&GDK&&7777	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8!!#####s   AA	Az)args, kwargs, exp_u_ind, exp_u_dep, exp_uc                     | j         j        |i | | j         j        d d          |k    sJ | j         j        d d          |k    sJ | j         j        d d          |k    sJ |                     d           || j         _        || j         _        | j         j        d d          |k    sJ | j         j        d d          |k    sJ | j         j        d d          |k    sJ |                     d           d S )N)r+   r,   r-   r   )r    rJ   r+   r,   r-   r<   )r"   r$   r   r   	exp_u_ind	exp_u_depexp_us          r#   test_speedszTestSystem.test_speeds   s     	////{ #y0000{ #y0000{}QQQ5((((  )@ AAA%%{ #y0000{ #y0000{}QQQ5((((  )@ AAAAAr%   zargs, kwargs, exp_u_auxc                     | j         j        |i | | j         j        d d          |k    sJ |                     d           || j         _        | j         j        d d          |k    sJ |                     d           d S )N)r.   r   )r    rL   r.   r<   )r"   r$   r   r   	exp_u_auxs        r#   test_auxiliary_speedsz TestSystem.test_auxiliary_speeds   s     	)($9&999{ #y0000   444%{ #y0000   44444r%   c                     t          j        t                    5   | j        j        |i | d d d            n# 1 swxY w Y   |                                  d S rz   )ru   r   r   r    rL   re   r"   rY   r   r   s       r#   test_auxiliary_invalidz!TestSystem.test_auxiliary_invalid   s     ]:&& 	> 	>,DK,d=f===	> 	> 	> 	> 	> 	> 	> 	> 	> 	> 	> 	> 	> 	> 	>!!#####   9= =zprop, add_func, args, kwargsr(   r)   r+   r,   r.   rL   r/   rK   r0   rN   r1   rO   r3   rm   r   r2   	add_loadsPNr5   add_actuatorsTAc                    t          | j        |d           |ddf}|dv r|dz  }|                     |           t          t	          | j        |          d d                    g k    sJ  t	          | j        |          |i | t          t	          | j        |          d d                    t          |          k    sJ d S )Nr&   r*   r-   )r0   r1   rQ   r   )setattrr    re   listr7   )r"   rY   propadd_funcr   r   r8   s          r#   test_add_after_resetzTestSystem.test_add_after_reset   s    $ 	T2&&&c"HHH00G!!'!222GDK..qqq122b8888&X&&7777GDK..qqq122d4jj@@@@@@r%   zprop, add_func, value, error   c                 F   t          j        |          5   t          | j        |          |           d d d            n# 1 swxY w Y   t          j        |          5  t	          | j        ||           d d d            n# 1 swxY w Y   |                                  d S rz   )ru   r   r7   r    r   re   )r"   rY   r   r   valueerrors         r#   test_type_errorzTestSystem.test_type_error  s     ]5!! 	2 	2*GDK**5111	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2]5!! 	. 	.DKu---	. 	. 	. 	. 	. 	. 	. 	. 	. 	. 	. 	. 	. 	. 	.!!#####s#   A  AABB	Bzargs, kwargs, exp_kdesr&   c                     g | ]
\  }}||z
  S r&   r&   r^   s      r#   rb   zTestSystem.<listcomp>#  s     ===wr3"s(===r%   rF   c                     g | ]
\  }}||z
  S r&   r&   r^   s      r#   rb   zTestSystem.<listcomp>%  s     	5	5	5wr3"s(	5	5	5r%      c                     | j         j        |i | |                     d           | j         j        d d          |k    sJ || j         _        |                     d           | j         j        d d          |k    sJ d S )N)r/   r   )r    rK   re   r/   )r"   rY   r   r   exp_kdess        r#   	test_kdeszTestSystem.test_kdes"  s     	d-f---!!)!444{"h....#!!)!444{"h......r%   c                     t          j        t                    5   | j        j        |i | d d d            n# 1 swxY w Y   |                                  d S rz   )ru   r   r   r    rK   re   r   s       r#   test_kdes_invalidzTestSystem.test_kdes_invalid1  s     ]:&& 	2 	2 DK $1&111	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2!!#####r   zargs, kwargs, exp_conc                    d}d |D             | j         z   } | j        j        |i | |                     |           | j        j        d d          |k    sJ | j        j        d d          |k    sJ || j        _        |                     |           | j        j        d d          |k    sJ | j        j        d d          |k    sJ d S )N)r0   rQ   c                 B    g | ]}|                     t                    S r&   )rk   rl   )r_   cs     r#   rb   z9TestSystem.test_holonomic_constraints.<locals>.<listcomp>C  s"    222Qqvvayy222r%   r   )rS   r    rN   re   r0   rQ   r"   rY   r   r   exp_conr8   exp_vel_cons          r#   test_holonomic_constraintsz%TestSystem.test_holonomic_constraints;  s     D22'222TX=--t>v>>>!!'!222{03w>>>>{/2kAAAA,3)!!'!222{03w>>>>{/2kAAAAAAr%   c                     t          j        t                    5   | j        j        |i | d d d            n# 1 swxY w Y   |                                  d S rz   )ru   r   r   r    rN   re   r   s       r#   "test_holonomic_constraints_invalidz-TestSystem.test_holonomic_constraints_invalidO  s     ]:&& 	C 	C1DK14B6BBB	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	C!!#####r   c                    d}| j         d t          | j                           |z   } | j        j        |i | |                     |           | j        j        d d          |k    sJ | j        j        d d          |k    sJ || j        _        |                     |           | j        j        d d          |k    sJ | j        j        d d          |k    sJ d S )N)r1   rQ   r   )rR   lenrP   r    rO   re   r1   rQ   r   s          r#   test_nonholonomic_constraintsz(TestSystem.test_nonholonomic_constraintsZ  s     Ggms47||m,w600$A&AAA!!'!222{3AAA6'AAAA{/2kAAAA/6,!!'!222{3AAA6'AAAA{/2kAAAAAAr%   c                     t          j        t                    5   | j        j        |i | d d d            n# 1 swxY w Y   |                                  d S rz   )ru   r   r   r    rO   re   r   s       r#   %test_nonholonomic_constraints_invalidz0TestSystem.test_nonholonomic_constraints_invalidn  s     ]:&& 	F 	F4DK4dEfEEE	F 	F 	F 	F 	F 	F 	F 	F 	F 	F 	F 	F 	F 	F 	F!!#####r   zconstraints, expectedc                 ~    || j         _        |                     d           | j         j        d d          |k    sJ d S )Nr   r   )r    rQ   re   )r"   rY   constraintsexpecteds       r#   #test_velocity_constraints_overwritez.TestSystem.test_velocity_constraints_overwritez  sK     ,7(!!*C!DDD{/2h>>>>>>r%   c                 $   t           d         t           d         z
  | j        _        |                     d           | j        j        d d          t           d         t           d         z
  gk    sJ d | j        _        |                                  d S )NrD   rB   r   r   )rG   r    rQ   re   r~   s     r#   &test_velocity_constraints_back_to_autoz1TestSystem.test_velocity_constraints_back_to_auto  s    +-a52a5=(!!*C!DDD{/2r!ur!u}oEEEE+/(!!#####r%   c                 &    t          d          t          d          }}t          d          t          d          c} j                            ||            j        j        g  j        ||R k    sJ  j                                        j        j        g  j        ||R k    sJ g  j        _         j        j        dk    sJ  j        _         j        j        fk    sJ t          d          t          j        t           fd           t          j        t           fd           t          j        t                    5  |||f j        _        d d d            n# 1 swxY w Y    j        j        fk    sJ d S )	Nrb1rb2p1p2r&   symbc                  8     j                                       S rz   r    rm   )r"   r   s   r#   <lambda>z(TestSystem.test_bodies.<locals>.<lambda>  s    )?)?)E)E r%   c                  8    j                                        S rz   r   )r   r"   s   r#   r   z(TestSystem.test_bodies.<locals>.<lambda>  s    $+*@*@*D*D r%   )
r   r   r    rm   r3   r   ru   r   r   r   )r"   rY   r   r   r   r   r   s   `    @@r#   test_bodieszTestSystem.test_bodies  s   U##Yu%5%5S$$BsB'''{!%<t{%<C%<%<%<<<<<r"""{!%@t{%@C%@%@R%@%@@@@@{!R''''{!bU****vi!E!E!E!E!EFFFj"D"D"D"D"DEEE]9%% 	: 	:"%sBD!9DK	: 	: 	: 	: 	: 	: 	: 	: 	: 	: 	: 	: 	: 	: 	:{!bU******s   E33E7:E7c                    t                      t          d          t          d          c}t          d          }t          d          }t	          d|          }                    t          |j                  ||j        ft          ||j                             j	        j        f||j        f||j        ffk    sJ ||j        fg_	        j	        ||j        ffk    sJ t          j        t          fd           t          j        t                    5  j        f_	        d d d            n# 1 swxY w Y   j	        ||j        ffk    sJ d S )Nr   r   r   )r   mc1r   c                  H                           j         j        f          S rz   )r   rp   y)r   r    s   r#   r   z+TestSystem.test_add_loads.<locals>.<lambda>  s    &*:*:AqsAC=*I*I r%   )r   r   r   r   r   r   r   rp   r
   r2   ru   r   r   r   )r"   r   r   r   r   r   r    s        @@r#   test_add_loadszTestSystem.test_add_loads  s   c""N3$7$71Q'''ElldC  QS))C:uR~~FFF|AC3*sACjAAAAAACz|AC{****j"I"I"I"I"IJJJ]9%% 	$ 	$qs8FL	$ 	$ 	$ 	$ 	$ 	$ 	$ 	$ 	$ 	$ 	$ 	$ 	$ 	$ 	$|AC{******s   $D??EEc                 x   t                      }t          d          t          d          }}t          t          d          |j        |          }t          t          d          |j        ||          }|                    |           |j        |fk    sJ |j        dk    sJ |f|_        |j        |fk    sJ d S )Nr   r   T1T2r&   )	r   r   r   r   rp   r   r   r5   r2   )r"   r    r   r   act1act2s         r#   test_add_actuatorszTestSystem.test_add_actuators  s    c""N3$7$71gdmmQS!44gdmmQS!Q77T"""D7****|r!!!! 7D7******r%   c           	      	   t          d          \  }}}}}}}t          dt                    \  }}	}
}t          d|||          }t	          d||	||          t          d|	|
||          }t          d|
|||                    t                              }t                                          |           j	        |fk    sJ j
        |fk    sJ j        t          |g          k    sJ j        t          |g          k    sJ j        t          ||                    t                    z
  g          k    sJ                     |
                               |                               ||                    t                    z
                                 |           j	        ||fk    sJ j
        ||
|	fk    sJ j        t          ||g          k    sJ j        t          ||g          k    sJ j        t          ||                    t                    z
  ||                    t                    z
  g          k    sJ                     ||                    t                    z
                                             j	        ||fk    sJ j
        ||
|	fk    sJ j        t          |||g          k    sJ j        t          |||g          k    sJ j        t          ||                    t                    z
  ||                    t                    z
  ||                    t                    z
   g          k    sJ                     |           j	        |||fk    sJ j
        ||
|	|fk    sJ j        t          ||||g          k    sJ j        t          ||||                    t                    g          k    sJ j        t          ||                    t                    z
  ||                    t                    z
  ||                    t                    z
   g          k    sJ j        d d          g k    sJ j        d d          g k    sJ t)          j        t,          fd           t)          j        t.          fd	           d S )
Nz	q1:5 u1:4zrb1:6r>   r@   rA   rC   J_lagc                  .                                    S rz   rH   )rA   r    s   r#   r   z,TestSystem.test_add_joints.<locals>.<lambda>  s    &*;*;B*?*? r%   c                  .                                    S rz   r   )r   r    s   r#   r   z,TestSystem.test_add_joints.<locals>.<lambda>  s    ):):3)?)? r%   )r   r   r   r   r   rk   rl   r   rH   r4   r3   r(   r	   r+   r/   rm   rI   rK   r)   r,   ru   r   r   r   )r"   q1q2q3q4u1u2u3r   rb3rb4rb5r@   rC   r   rA   r   r    s                  @@@r#   test_add_jointszTestSystem.test_add_joints  s   %3K%@%@"BBB")'y"A"A"AS#sCdCb"--D#sB33dCb"--#sB

;;"}%%%%}c
****|t444444|t444444{orBGGAJJ.?@@@@@@#r"""RWWQZZ((("}R((((}c3 44444|Bx888888|Bx888888{o"''!**_b2771::o.0 0 0 0 0 0"rwwqzz/*+++"}R,,,,}c3 44444|B|<<<<<<|B|<<<<<<{orBGGAJJRWWQZZ13bggajj/A/C D D D D D D%   }RU 33333}c3S 99999|BB/?@@@@@@|BBGGAJJ/GHHHHHH{orBGGAJJRWWQZZ13bggajj/A/C D D D D D D|AAA"$$$$|AAA"$$$$j"?"?"?"?"?@@@i!?!?!?!?!?@@@@@r%   c                    | j         dd          | j        _         | j        j         | j         dd          k    sJ |                     d           d| j        _        d| j        _        | j         | j        _         |                                  d S )Nr   )r4   r   r&   )r4   r    re   r(   r+   r~   s     r#   test_joints_setterzTestSystem.test_joints_setter  s    ![_{!T[_4444!!+!666![!!#####r%   zname, joint_index))r@   r   )rA   r   not_existingNc                 p    | j                             |          }||J d S || j        |         k    sJ d S rz   )r    	get_jointr4   )r"   rY   r   joint_indexjoints        r#   test_get_jointzTestSystem.test_get_joint  sJ     %%d++=====DK4444444r%   zname, body_index))r   r   )r   rB   r  c                 p    | j                             |          }||J d S || j        |         k    sJ d S rz   )r    get_bodyr3   )r"   rY   r   
body_indexr   s        r#   test_get_bodyzTestSystem.test_get_body  sJ     {##D))<<<<<4;z2222222r%   r6   c                      G d d|          }| j                             |           t          | j         j        |          sJ d S )Nc                       e Zd ZdS ):TestSystem.test_form_eoms_calls_subclass.<locals>.MyMethodN)rr   rs   rt   r&   r%   r#   MyMethodr    s        Dr%   r  )r6   )r    	form_eomsr   r6   )r"   rq   r6   r  s       r#   test_form_eoms_calls_subclassz(TestSystem.test_form_eoms_calls_subclass  sh    	 	 	 	 	z 	 	 	 	222$+0(;;;;;;;r%   zkwargs, expectedrh   explicit_kinematicsc                 N     | j         j        di | | j         j        |k    sJ d S )Nr&   )r    r  mass_matrix_full)r"   rq   r   r   s       r#   !test_system_kane_form_eoms_kwargsz,TestSystem.test_system_kane_form_eoms_kwargs  s;     	'''''{+x777777r%   zkwargs, mm, gmc                      | j         j        ddt          i| | j         j        |k    sJ | j         j        |k    sJ d S )Nr6   r&   )r    r  r   r  forcing_full)r"   rq   r   mmgms        r#   %test_system_lagrange_form_eoms_kwargsz0TestSystem.test_system_lagrange_form_eoms_kwargs  sV     	CCCFCCC{+r1111{'2------r%   zeom_method, kwargs, errornon_existing_kwargkd_eqs
Lagrangianc                    t           d         | j        _        t          dt	          d                    }| j                            |           |j                            | j        j        t           d         | j        j	        z             t          j        |          5   | j        j        dd|i| d d d            d S # 1 swxY w Y   d S )Nr   rg   rh   ri   r6   r&   )r*   r    r(   r   r   rm   rn   ro   r   rp   ru   r   r  )r"   r$   r6   r   r   rg   s         r#   test_form_eoms_kwargs_errorsz'TestSystem.test_form_eoms_kwargs_errors!  s    aDSws||,,,q!!!	T[4adT[]6JKKK]5!! 	C 	C!DK!BBZB6BBB	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	C 	Cs   !CC
C)8rr   rs   rt   r|   r   ru   markparametrizer   r   r   r   r   r*   r   r-   r   r   r   rM   r   r   rG   r   r
   rp   r   r   r   r   r   rc   r   r   r   r   r   r   r   r   r   r   r   r   r   r  r  r   r   r  r	   r  rk   rl   r  r   r&   r%   r#   rx   rx   k   s       & & && & & [Wt^^G-D-D&EFF[]T553G3G,HIIQ Q JI GFQ0# # #& & &
 [H	
2A2AbqbE2q!u%	
2A2%q!ub!BQB%8	
2A2&AbqbE1RaR59	
2A2 3 3 34qtQqTlQqTF
A$!ad			K  B B B  [V&7%FGG[^
A$!r
A$!r
A$=%()
A$=%()
A$!r
'#,,!	r".  $ $  HG$ [H	
2A2AbqbE2q!u%	
2A2%q!ub!BQB%8	
2A2&AbqbE1RaR59	
2A2 3 3 34qtQqTlQqTF
A$!ad			K  B B B  [6	BQBRV9  	5 	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"3!5r:	+eeCjj..2E2E2G H HJBO	oGCLL..--/1D1D)F )F (HIK	M>   	A 	A!  	A [;	#WWS\\:>	#WWS\\:>	,j9	,j9	(''#,,
C	Q	*	 "=q)L	#%CQ			<y9	+wws||Y7	owws||Y?>  $ $ $ [5	R==##aeRV*<*<===>
A$A,!r!u	%r	5	5##aeRV"4"4	5	5	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 
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 
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 *A *AX$ $ $ [0 3 3 3  
5 5 
5 [/ 2 2 2  
3 3 
3 [\K+IJJ< < KJ< [/	__r1g773<<'89::;
	&!Q:;WWS\\9J9L )M )M 	N2  
8 8 
8
 [-	__q!fq''#,,&7899	!A$))A,,*	+	+	-0  . .	 . [8	+Q/;	/3Y?	xnj1	xnj1	8R.*5	<+Z8;  C C C C Cr%   rx   c                   >   e Zd Zej                            deedfeedfg          d             Zej                            dedfedfg          d             Z	d Z
d Zd	 Zej                            dedfedfg          d
             Zd Zd Zd Zd Zd ZdS )TestValidateSystemz)valid_method, invalid_method, with_speedsTFc                     |                      |           | j                            |           t          j        t
                    5  | j                            |           d d d            d S # 1 swxY w Y   d S )NrW   )rU   r    r{   ru   r   r   )r"   valid_methodinvalid_methodrT   s       r#   test_only_validz"TestValidateSystem.test_only_valid4  s    
 	""{";;;##L111]:&& 	8 	8K''777	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8 	8s   
A22A69A6zmethod, with_speedsc                                           |            j        dd           j        _         j        d d          j        _         j        d d          j        _        t          j        t           fd           d S )NrW   r   r  c                  8    j                                        S rz   r    r{   methodr"   s   r#   r   zBTestValidateSystem.test_missing_joint_coordinate.<locals>.<lambda>F      $+*E*Ef*M*M r%   )rU   r(   r    r+   r/   ru   r   r   r"   r-  rT   s   `` r#   test_missing_joint_coordinatez0TestValidateSystem.test_missing_joint_coordinate?  s~     	""{";;; JqrrN JssO9SbS>j"M"M"M"M"MNNNNNr%   c                       j         d d          j        _          j        dd           j        _         j        d d          j        _        t	          j        t           fd           d S )Nr  r   c                  6     j                                         S rz   r+  r!   s   r#   r   z=TestValidateSystem.test_missing_joint_speed.<locals>.<lambda>L      $+*E*E*G*G r%   )r(   r    r+   r/   ru   r   r   r~   s   ` r#   test_missing_joint_speedz+TestValidateSystem.test_missing_joint_speedH  sa     JssO JqrrN9SbS>j"G"G"G"GHHHHHr%   c                 t      j         dd           j        _         t          j        t           fd           d S )Nr   c                  6     j                                         S rz   r+  r!   s   r#   r   z<TestValidateSystem.test_missing_joint_kdes.<locals>.<lambda>P  r3  r%   )r/   r    ru   r   r   r~   s   ` r#   test_missing_joint_kdesz*TestValidateSystem.test_missing_joint_kdesN  s9    9QRR=j"G"G"G"GHHHHHr%   c                     | j         d          g| j         dd          z   | j        _         | j                                         d S )Nr   r   )r/   r    r{   r~   s     r#   test_negative_joint_kdesz+TestValidateSystem.test_negative_joint_kdesR  s?    !Yq\M?TYqrr]:##%%%%%r%   c                                           |           g  j        _         j         j        d          j        d         z
   j        d         z   gz    j        _        t          j        t           fd           g  j        _
         j         j
        z    j        _         j                                       d S )NrW   r   r   c                  8    j                                        S rz   r+  r,  s   r#   r   zFTestValidateSystem.test_missing_holonomic_constraint.<locals>.<lambda>]  r.  r%   )rU   r    r0   rS   r+   r,   r1   ru   r   r   r)   r(   r{   r/  s   `` r#   !test_missing_holonomic_constraintz4TestValidateSystem.test_missing_holonomic_constraintV  s     	""{";;;,.)/3xJqMDJqM)DJqM9;; 0;,j"M"M"M"M"MNNN J3##F+++++r%   c                      g  j         _        t          j        t           fd            j        d          j         _         j         j        d         gz    j         _         j                                          d S )Nc                  6     j                                         S rz   r+  r!   s   r#   r   zITestValidateSystem.test_missing_nonholonomic_constraint.<locals>.<lambda>d  r3  r%   r   r   )r    r1   ru   r   r   r,   r+   r{   r~   s   ` r#   $test_missing_nonholonomic_constraintz7TestValidateSystem.test_missing_nonholonomic_constraintb  sl    /1,j"G"G"G"GHHH JqM J$*Q-8##%%%%%r%   c                      j         t          d         gz    j        _          j        t          d         t          d         z
  gz    j        _         j                                          j         j        _         j         d d          j        _          j        d d          j        _        t          j        t           fd           d S )Nr   r  c                  6     j                                         S rz   r+  r!   s   r#   r   zFTestValidateSystem.test_number_of_coordinates_speeds.<locals>.<lambda>r  r3  r%   )
r+   r-   r    r/   rG   r{   r(   ru   r   r   r~   s   ` r#   !test_number_of_coordinates_speedsz4TestValidateSystem.test_number_of_coordinates_speedsi  s     J!A$/9!r!u~5##%%% J JssO9SbS>j"G"G"G"GHHHHHr%   c                 &     j         d d          j        _         t          j        t           fd            j         t
          d         t
          d         z   t          d         z
  gz    j        _         t          j        t           fd           d S )Nr  c                  6     j                                         S rz   r+  r!   s   r#   r   z8TestValidateSystem.test_number_of_kdes.<locals>.<lambda>w  r3  r%   rB   r   c                  6     j                                         S rz   r+  r!   s   r#   r   z8TestValidateSystem.test_number_of_kdes.<locals>.<lambda>y  r3  r%   )r/   r    ru   r   r   r-   rG   r~   s   ` r#   test_number_of_kdesz&TestValidateSystem.test_number_of_kdest  s~    9SbS>j"G"G"G"GHHH9!qtbe(;'<<j"G"G"G"GHHHHHr%   c                 <    | j                             d           d S )NT)check_duplicatesr+  r~   s     r#   test_duplicatesz"TestValidateSystem.test_duplicates{  s!    ##T#:::::r%   c           	         t           d t          | j                           | j        _        t	          j        t                    5  | j                            t                     d d d            n# 1 swxY w Y   g | j        _        | j                            t                     t          | j        _
        t	          j        t                    5  | j                            t                     d d d            n# 1 swxY w Y   g | j        _
        | j                            t                     | j                            t          dt          d          t          d                               g | j        _        t	          j        t                    5  | j                            t                     d d d            d S # 1 swxY w Y   d S )NJurbu1rbu2)r-   r   r+   r    ru   r   r   r{   r   rM   r.   rH   r   r   )r"   r[   s     r#   test_speeds_in_lagrangez*TestValidateSystem.test_speeds_in_lagrange  s/   .s4:./]:&& 	9 	9K''888	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9##O444]:&& 	9 	9K''888	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9##O444T9V,,i.?.?@@	B 	B 	B]:&& 	9 	9K''888	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9 	9s6    A11A58A5 C==DD GGGN)rr   rs   rt   ru   r!  r"  r   r   r(  r0  r4  r7  r9  r<  r?  rB  rF  rI  rN  r&   r%   r#   r$  r$  3  st       [H	ot,	+u-K  8 8	 8 [2	dou557 8 8O O8 8OI I II I I& & & [2	dou557 8 8, ,8 8,& & &	I 	I 	II I I; ; ;9 9 9 9 9r%   r$  c                        e Zd Zd Zd Zd ZdS )TestSystemExamplesc                    t          d          \  }}}}}t          d          \  }}}}	}
t          d          }t          d|          }t          d|          }t	          d          }t          j        |          }|j        |fk    sJ |j        |j        k    sJ |j	        |j
        k    sJ t          d||||
|j        	          }t          d
||||	|j        |||j        z            }|                    ||           |j        ||fk    sJ |                    d          |k    sJ |                    d          |k    sJ |                    | |j        z             |                    |j
        ||j        z  f           |                    t/          ||z  |j        ||                     |                                 |                                 t5          |j        t8                    sJ t;          |j        t?          ||z   ||z  tA          |          z  g||z  tA          |          z  ||dz  z  gg          z
            tC          dd          k    sJ t;          |j"        t?          ||z  |	dz  z  tG          |          z  |z   g| |z  |z  tG          |          z  ||z  z   gg          z
            tC          dd          k    sJ |$                    tK          |j
        &                    |j
                  '                    |j                                       |j        J ||c|_(        |_)        |	|
c|_*        |_+        |                                 || tG          |          z  |
| tA          |          z  |	z  |
,                    tZ                    ||	dz  tG          |          z  |	,                    tZ                    tA          |          z  z
  z  i}| |z  tG          |          z  ||z  |z  z   ||z  tG          d|z            z  |	dz  z  dz  z   ||z  tG          d|z            z  |	dz  z  dz  z
  |tA          |          z  z
  ||tA          |          dz  z  |tG          |          dz  z  z   z  z  }t]          t_          |                                0                    |          |	,                    tZ                              1                                          d         }t;          ||z
            dk    sJ t5          |j        t8                    sJ t?          ddgddgg           }t?          |
|	g           }t?          |dz   |z  tA          |          dz  z  |dz  |z  z   ||z  tA          |          z  |||z   z  tA          |          z  z
  g|tA          |          z  dgg          }t?          | |z  |z  tG          |          z  ||z  z   |dz  |z  |	dz  z  tG          |          z  tA          |          z  z
  ||z  tA          |          z  z
  g||	dz  z  tG          |          z  gg          }|2                    tC          dd                    3                    tC          dd          2                    |                    }|3                    |          }t;          |j        |z
            tC          dd          k    sJ t;          |j"        |z
            tC          dd          k    sJ t;          |j4        |z
            tC          dd          k    sJ t;          |j5        |z
            tC          dd          k    sJ d S )Ng l mc mp kzF qp qc up ucrailcartri   bob	bob_frameslider
joint_axispinrY  child_interframechild_pointrB   r   r   rF   )6r   r   r   r   r   r   r   r3   r   r   rn   r   rp   r   zr   rH   r4   r  r	  apply_uniform_gravityr   r   r   r{   r  r   r6   r   r   mass_matrixr	   r   r   forcingr   rN   r   pos_fromdotr(   r)   r+   r,   rk   rl   rd   r   xreplacevaluesrow_joincol_joinr  r  )r"   glmcmpkFqpqcupucrS  rT  rU  rV  r    rW  rZ  subsupd_expectedupd_solMkgkMdgdMmr  s                              r#   test_cart_pendulum_kanesz+TestSystemExamples.test_cart_pendulum_kanes  s    "-001b"a*?;;2r2r  b)))u2&&&";//	&t,,}''''|tz))))!T_4444$b"PPPudCRDF(1q9;P P P&#&&&}----))V3333u%%,,,,$$aR&(]333$/1tv:6777^AFDFItLLMMM   &+[99999+o2grAvB'(26CGG+;R!q&[*IJ/L /L L M MA;;    /!VbAgB'!+,S1Wq[3r77"QV+,;. +/ +/ / 0 038A;;? ? ? ? 	((CN++DO<<@@JJKK	M 	M 	M (((%'"fl%'"fl    QBRLQBRL2%

Aq3r77!2RWWQZZ#b''5I!IJL R"Ws2wwR!+a"fs1r6{{.BR1W.Lq.PP2vAF#bAg-1245BK@"s2ww!|#b3r77a<&778: 	 f..0099$?? ggajj* **0&((4 4457,.//14444&+[99999 1v1v.///r2h'''Qw|c"ggl:Q!Vb[H 2vB/!rBw-#b''2IIK 3r77{A.0 1 1 b1frkCGG#a"f,qAv{R1W/Ds2ww/N"gg0 AB( )+,rQw;R+@*ACD D kk%1++&&00q!1E1Eb1I1IJJ[[__*R/00E!QKK????+,,a;;;;/"455q!DDDD+b011U1a[[@@@@@@r%   c                 V   t          d          \  }}}}}t          d          \  }}}t          dd          \  }	}
t          d          }t          d|          }t          d|          }t	          d	          }t          j        |          }|j        |fk    sJ |j        |j        k    sJ |j	        |j
        k    sJ t          d
||||
|j                  }t          d||||	|j        |||j        z            }|                    ||           |j        ||fk    sJ |                    d
          |k    sJ |                    d          |k    sJ |j        D ]I}|j        |z  |j
                            |j	                                      |j                  z  |_        J|                    |j
        ||j        z  f           |                    t5          ||z  |j        ||                     |                    t8                     |                    t8                     t=          |j        tA          ||z   ||z  tC          |          z  g||z  tC          |          z  ||dz  z  gg          z
            tE          dd          k    sJ t=          |j#        tA          ||z  |	dz  z  tI          |          z  |z   g| |z  |z  tI          |          z  ||z  z   gg          z
            tE          dd          k    sJ |%                    tM          |j
                            |j
                                      |j                                       |j'        J ||c|_(        |_)        || tI          |          z  |
| tC          |          z  |	z  |
*                    tV                    ||	dz  tI          |          z  |	*                    tV                    tC          |          z  z
  z  i}| |z  tI          |          z  ||z  |z  z   ||z  tI          d|z            z  |	dz  z  dz  z   ||z  tI          d|z            z  |	dz  z  dz  z
  |tC          |          z  z
  ||tC          |          dz  z  |tI          |          dz  z  z   z  z  }|                    t8                    }|j'        j,        d         }|j'        -                                |         }t]          |d         /                    ||i          /                    |          |	*                    tV                              d         }t=          ||z
            dk    sJ ta          |j'        t8                    sJ tA          |dz  |z  ||z  tC          |          z  | tC          |          z  g||z  tC          |          z  ||z   dgg          }tA          | |z  |z  tI          |          z  ||z  z   g||z  tI          |          z  |	dz  z  |z   gg          }tc          d          2                    tE          dd                    3                    tE          dd          2                    |3                    tA          |tC          |          z  ddg          j4                                      }tA          |	|
g|d d          z   |tI          |          z  |	dz  z  gz             }t=          |j        |z
            tE          dd          k    sJ t=          |j#        |z
            tE          dd          k    sJ t=          |j5        |z
            tE          dd          k    sJ t=          |j6        |z
            tE          dd          k    sJ d S )NrR  zF qp qczqp qcr   rS  rT  ri   rU  rV  rW  rX  rZ  r[  rB   r   r  rD   r   )7r   r   r   r   r   r   r   r3   r   r   rn   r   rp   r   r^  r   rH   r4   r  r	  rj   rb  rc  potential_energyr   r   r   r{   r   r  r   r`  r	   r   r   ra  r   rN   r   r6   r(   r)   rk   rl   lam_vecsolve_multipliersr   rd  r   r   rf  rg  r   r  r  )r"   rh  ri  rj  rk  rl  rm  rn  ro  qpdqcdrS  rT  rU  rV  r    rW  rZ  r   rr  qpdd_expectedeomslam1lam1_solqpdd_solrw  rx  ry  r  s                                r#   test_cart_pendulum_lagrangez.TestSystemExamples.test_cart_pendulum_lagrange  s    "-001b"a"9--	2r!'1--S  b)))u2&&&";//	&t,,}''''|tz))))!T_4444$b#+/63 3 3udCSTV(1q9;P P P&#&&&}----))V3333u%%,,,,M 	2 	2D$(IMDO4L4L"5$ 5$$'CMM%2D!!$/1tv:6777^AFDFItLLMMM///)))+o2grAvB'(26CGG+;R!q&[*IJ/L /L L M MA;;    /!VcQhR(1,-a!c"gg0EB0N/O;Q +
 +
 
  Aqkk    	((CN++DO<<@@JJKK	M 	M 	M (((%'"fl QBRLaR#b''\C'Q#(SWW"4sxx{{SWW7L"LMO R"Ws2wwR!+a"fs1r6{{.BSAX.M/ R#a"f++%q0145783r77{C"s2ww!|#b3r77a<&778: 	 00 (+$6688>a))4*:;;DDTJJ!& &&')=011Q6666&+_===== qAv{AFSWW,<qb3r77lK 2vB/b"=? @ @b1frkCGG#a"f,-"fs2ww)A-.01 1 !ffooeAqkk**44U1a[[5I5IKKSWWa(;<<>??6A 6A B Bc3Z"QQQ%/1s2ww;3I2JJKK*R/00E!QKK????+,,a;;;;/"455q!DDDD+b011U1a[[@@@@@@r%   c           	         t          d          \  }}}t          d          \  }}}t          dd          \  }}t          d|          }	t                      }
|
                    |	           |	j                            |
j        ||
j        z             |	j        	                    |
j
        ||
j        z  ||
j        z  z              |g|g|gc|
_        |
_        |
_        |                    t                     |z
  g|
_        |
                    | |
j        z             |
                    t)          |	||
j        z            t)          |	||
j        z  ||z  |
j        z  z
                       |
                                 |
                                 t/          dg          }t/          |g          }t/          |g          }t/          |||z  z
  g          }|                    t3          dd                                        t3          dd                              |                    }|                    |          }t/          |||z  z
  g          }t7          |
j        |z
            t3          dd          k    sJ t7          |
j        |z
            t3          dd          k    sJ t7          |
j        |z
            t3          d	d	          k    sJ t7          |
j        |z
            t3          d	d          k    sJ t7          |
j         j!        |z
            t3          dd          k    sJ d S )
Nzg m muzq u uazN FT)positiver   ri   r   rB   )"r   r   r   r   rm   rn   ro   r   rp   set_velr   r   r(   r+   r.   rk   rl   r/   r_  r   r
   r{   r  r	   rf  r   rg  r   r`  ra  r  r  r6   auxiliary_eqs)r"   rh  rh   mur*   r-   rM   r   rm  r   r    ru  rv  rw  rx  ry  r  aux_eqss                     r#   test_box_on_groundz%TestSystemExamples.test_box_on_ground  s    8$$1b!(++1bed3331Sq!!!!	V/VX>>>	V\1vx<"vx-+GHHH453bT0flFLvvayy1}o$$aR&(]333!Q\""!Q\BFVX$5566	8 	8 	8 	    aS!!aS!!aS!!a"q&j\**kk%1++&&00q!1E1Eb1I1IJJ[[__!1q1u9+..*R/00E!QKK????+,,a;;;;/"455q!DDDD+b011U1a[[@@@@)7'A  "1a[[) ) ) ) ) )r%   N)rr   rs   rt   rz  r  r  r&   r%   r#   rP  rP    sO        CA CA CAJEA EA EAN!) !) !) !) !)r%   rP  ))ru   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   _trl   r*   rG   r-   rM   r   rx   r$  rP  r&   r%   r#   <module>r     sY    % % % % % % & & & & & & = = = = = = = = + + + + + + + + 4 4 4 4 4 4                              - , , , , , ' ' ' ' ' 'N5^E1N5^FRL RL RL RL RL RL RL RLjEC EC EC EC EC EC EC ECP[9 [9 [9 [9 [9 [9 [9 [9|n) n) n) n) n) n) n) n) n) n)r%   