
    sgK                     R   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(y)    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y)TestSystemBasec                 J    t        t        d      t        d            | _        y )Nframefixed_point)r   r   r   systemselfs    b/var/www/html/venv/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_system_class.py_empty_system_setupz"TestSystemBase._empty_system_setup   s    ^G4eM6JK    c                     d}d}|D ]%  }||vst        | j                  |      d d  g k(  r%J  |D ]"  }||vst        | j                  |      dk(  r"J  d|vr| j                  j                  J y y )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    r6   )r"   excludematricestuplesattrs        r#   _empty_system_checkz"TestSystemBase._empty_system_check   s    Q; 	;D7"t{{D1!4:::	;  	8D7"t{{D1R777	8 w&;;))111 'r%   c           
      J   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                  j                  t        d   dg       | j                  j                  |d   d       |rR| j                  j!                  |d   t
        d   z
         | j                  j#                  t$        d   t$        d          | j                  j'                  t        d
   t        d   z
  t        d   z          | j                  j)                  |d   t
        d   z
  |d
   z          |d d
 | j                  _        |d
d | j                  _        | j                  j.                  d d  | j                  j0                  d d  c| _        | _        | j                  j*                  d d  | j                  j,                  d d  c| _        | _        | j                  j2                  d d  | _        | j                  j4                  d d  | _        | j                  j8                  d d  | _        | j                  j<                  d d  | _        y )Nr   r   r   zrb1:5clsJ1r   r   J2   J3   F)independent   ) r   r   r   r    r   qdr   r   r2   r   r)   r   r3   
add_jointsadd_coordinates
add_speedsadd_kdesadd_auxiliary_speedsuaadd_holonomic_constraintsadd_nonholonomic_constraintsr*   r+   r'   r(   r.   r/   hcvelocity_constraintsvcr0   nhc)r"   with_speedsr,   s      r#   _create_filled_systemz$TestSystemBase._create_filled_system(   sl   ^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                 (    | j                  d       y )NTrT   rU   r!   s    r#   _filled_system_setupz#TestSystemBase._filled_system_setupB   s    ""t"4r%   c                 (    | j                  d       y )NFrW   rX   r!   s    r#   _filled_system_setup_no_speedsz-TestSystemBase._filled_system_setup_no_speedsF   s    ""u"5r%   c                 f   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  t        t
        d d t        d d       D cg c]
  \  }}||z
   c}}k(  sJ d|v s:| j                   j                  d d  t        d   t        d   z
  t        d   z   gk(  sJ d|v s:| j                   j                  d d  t
        d   t        d   z
  t
        d   z   gk(  sJ d|v sV| 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 y y c c}}w )Nr'   rD   r(   r)   rF   r*   rB   r+   r,   r-   r.   r/   r   r   r0   rQ   r2   r3   )r    r'   r)   r(   r*   r,   r+   r-   rM   r.   ziprG   r/   r0   rQ   r2   tupler3   )r"   r8   uiqdis       r#   _filled_system_checkz#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 SBH<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   J-c                    t         d   | j                  _        t        d   | j                  _        t        d   t         d   j                  t              z
  | j                  _        t        dt        d            }| j                  j                  |       |j                  j                  | j                  j                  t         d   | j                  j                  z         y )Nr   pmmass)r)   r    r'   r,   r*   difftr.   r   r   
add_bodies
masscenterset_posr   x)r"   r$   rc   s      r#   _moving_point_massz!TestSystemBase._moving_point_massa   s    aDaDQ4!A$))A,.Sws|,q!	T[[44adT[[]]6JKr%   N)r5   )T)__name__
__module____qualname__pytestfixturer$   r<   rU   rY   r[   ra   rm   r5   r%   r#   r   r      s    V^^L L2;4 V^^5 5 V^^6 6:. V^^L Lr%   r   c                      e Zd Zd Zd Zej                  j                  dd ed      g      ej                  j                  dd e	d      g      d               Z
d Zd Zej                  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                  j                  dddg      ej                  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                  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                  j                  dedd
 i edd
 fg      d        Zej                  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                  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      j2                        fi fd3d4 e ed5       ed2      j2                   ed6            fi fg      d7        Zej                  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                  j                  d;d<i  eedd= edd=       D  cg c]
  \  }}||z
   c}}} fed=   ed=   z
  ed   ed   z
  fi  eedd> edd>       D  cg c]
  \  }}||z
   c}}} fg      d?        Z ej                  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      d@        Z!ej                  j                  dAd<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      dB        Z"ej                  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      dC        Z#ej                  j                  dAd<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                  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      dE        Z%ej                  j                  dFg 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      dG        Z&dH Z'dI Z(dJ Z)dK Z*dL Z+dM Z,ej                  j                  dNg dO      dP        Z-ej                  j                  dQg dR      dS        Z.ej                  j                  dTe/e0g      dU        Z1ej                  j                  dVi  e2dWdgd edX      gg      fdYdi e2ddgd edX      gg      fg      dZ        Z3ej                  j                  d[i  e2ddgd edX      gg       e2ed   ji                  e5      dg      fg      d\        Z6ej                  j                  d]e/d^diefe0d^diefe/d,g iefe/d_g iefe0d,g iefe0d`diefg      da        Z7yc c}}} w c c}}} w )b
TestSystemc                 X    | j                          | j                  j                          y N)r<   r    validate_system)r"   r$   s     r#   test_empty_systemzTestSystem.test_empty_systeml   s      "##%r%   c                 X    | j                          | j                  j                          y rv   )ra   r    rw   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 | j                  j                  |k(  sJ |%| j                  j                  j                  dk(  sJ | j                  j                  |k(  sJ | 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 y )Ninertial_pointinertial_frame)r   r    r   namer   r<   
isinstancer'   r	   r(   r)   r*   r+   r,   r.   r/   r0   )r"   r   r   s      r#   	test_initzTestSystem.test_initt   s    5= (DK 4DK;;**//3CCCC;;**k999=;;$$))-====;;$$---  "$++++_===$++++_===$++--999$++++_===$++++_===$++--999$++**O<<<$++;;_MMM$++>>PPPr%   c                 *   t        d      }t        j                  |      | _        | j                  j                  |j
                  k(  sJ | j                  j                  |j                  k(  sJ | j                  d       |f| j                  _        y )Nbody)r2   r8   )	r   r   from_newtonianr    r   rj   r   r<   r2   )r"   rbs     r#   test_from_newtonian_rigid_bodyz)TestSystem.test_from_newtonian_rigid_body   su    v++B/{{&&"--777{{  BHH,,,   5 Ur%   c                     t        d      }t        j                  t              5  t	        j
                  |       d d d        y # 1 sw Y   y xY w)Nparticle)r   rq   raises	TypeErrorr   r   )r"   pts     r#   test_from_newtonian_particlez'TestSystem.test_from_newtonian_particle   s;    j!]]9% 	&!!"%	& 	& 	&s   AAz)args, kwargs, exp_q_ind, exp_q_dep, exp_qrD   rE   TF)TFTr   rB   r   c                 0    | j                   j                  |i | | j                   j                  d d  |k(  sJ | j                   j                  d d  |k(  sJ | j                   j                  d d  |k(  sJ | 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 | j                  d       y )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{{  #y000{{  #y000{{}}Q5(((  )@ A%%{{  #y000{{  #y000{{}}Q5(((  )@ Ar%   funcrI   rJ   zargs, kwargs   ac                     t        j                  t              5   t        | j                  |      |i | d d d        | j                          y # 1 sw Y   xY wrv   )rq   r   
ValueErrorr7   r    ra   )r"   rY   r   r   r   s        r#   test_coordinates_speeds_invalidz*TestSystem.test_coordinates_speeds_invalid   sK     ]]:& 	8&GDKK&77	8!!#	8 	8   AAz)args, kwargs, exp_u_ind, exp_u_dep, exp_uc                 0    | j                   j                  |i | | j                   j                  d d  |k(  sJ | j                   j                  d d  |k(  sJ | j                   j                  d d  |k(  sJ | 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 | j                  d       y )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{{  #y000{{}}Q5(((  )@ A%%{{  #y000{{  #y000{{}}Q5(((  )@ Ar%   zargs, kwargs, exp_u_auxc                     | j                   j                  |i | | j                   j                  d d  |k(  sJ | j                  d       || j                   _        | j                   j                  d d  |k(  sJ | j                  d       y )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{{  #y000   4%{{  #y000   4r%   c                     t        j                  t              5   | j                  j                  |i | d d d        | j                          y # 1 sw Y   xY wrv   )rq   r   r   r    rL   ra   r"   rY   r   r   s       r#   test_auxiliary_invalidz!TestSystem.test_auxiliary_invalid   sJ     ]]:& 	>,DKK,,d=f=	>!!#	> 	>r   zprop, add_func, args, kwargsr'   r(   r*   r+   r-   rL   r.   rK   r/   rN   r0   rO   r2   ri   r   r1   	add_loadsPNr4   add_actuatorsTAc                 X   t        | j                  |d       |ddf}|dv r|dz  }| j                  |       t        t	        | j                  |      d d        g k(  sJ  t	        | j                  |      |i | t        t	        | j                  |      d d        t        |      k(  sJ y )Nr5   r)   r,   )r/   r0   rQ   r   )setattrr    ra   listr7   )r"   rY   propadd_funcr   r   r8   s          r#   test_add_after_resetz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                 2   t        j                  |      5   t        | j                  |      |       d d d        t        j                  |      5  t	        | j                  ||       d d d        | j                          y # 1 sw Y   OxY w# 1 sw Y   &xY wrv   )rq   r   r7   r    r   ra   )r"   rY   r   r   valueerrors         r#   test_type_errorzTestSystem.test_type_error  sw      ]]5! 	2*GDKK*51	2]]5! 	.DKKu-	.!!#		2 	2	. 	.s   BBB
Bzargs, kwargs, exp_kdesr5   rF      c                     | j                   j                  |i | | j                  d       | j                   j                  d d  |k(  sJ || j                   _        | j                  d       | j                   j                  d d  |k(  sJ y )N)r.   r   )r    rK   ra   r.   )r"   rY   r   r   exp_kdess        r#   	test_kdeszTestSystem.test_kdes"  s     	d-f-!!)!4{{"h...#!!)!4{{"h...r%   c                     t        j                  t              5   | j                  j                  |i | d d d        | j                          y # 1 sw Y   xY wrv   )rq   r   r   r    rK   ra   r   s       r#   test_kdes_invalidzTestSystem.test_kdes_invalid1  sJ     ]]:& 	2 DKK  $1&1	2!!#	2 	2r   zargs, kwargs, exp_conc                    d}|D cg c]  }|j                  t               c}| j                  z   } | j                  j                  |i | | j                  |       | j                  j                  d d  |k(  sJ | j                  j                  d d  |k(  sJ || j                  _        | j                  |       | j                  j                  d d  |k(  sJ | j                  j                  d d  |k(  sJ y c c}w )N)r/   rQ   r   )rg   rh   rS   r    rN   ra   r/   rQ   )r"   rY   r   r   exp_conr8   cexp_vel_cons           r#   test_holonomic_constraintsz%TestSystem.test_holonomic_constraints;  s     D*12Qqvvay2TXX=---t>v>!!'!2{{003w>>>{{//2kAAA,3)!!'!2{{003w>>>{{//2kAAA 3s   C=c                     t        j                  t              5   | j                  j                  |i | d d d        | j                          y # 1 sw Y   xY wrv   )rq   r   r   r    rN   ra   r   s       r#   "test_holonomic_constraints_invalidz-TestSystem.test_holonomic_constraints_invalidO  sN     ]]:& 	C1DKK114B6B	C!!#	C 	Cr   c                    d}| j                   d t        | j                         |z   } | j                  j                  |i | | j                  |       | j                  j                  d d  |k(  sJ | j                  j                  d d  |k(  sJ || j                  _        | j                  |       | j                  j                  d d  |k(  sJ | j                  j                  d d  |k(  sJ y )N)r0   rQ   r   )rR   lenrP   r    rO   ra   r0   rQ   )r"   rY   r   r   r   r8   r   s          r#   test_nonholonomic_constraintsz(TestSystem.test_nonholonomic_constraintsZ  s     Gggms477|,w60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                     t        j                  t              5   | j                  j                  |i | d d d        | j                          y # 1 sw Y   xY wrv   )rq   r   r   r    rO   ra   r   s       r#   %test_nonholonomic_constraints_invalidz0TestSystem.test_nonholonomic_constraints_invalidn  sN     ]]:& 	F4DKK44dEfE	F!!#	F 	Fr   zconstraints, expectedc                     || j                   _        | j                  d       | j                   j                  d d  |k(  sJ y )Nr   r   )r    rQ   ra   )r"   rY   constraintsexpecteds       r#   #test_velocity_constraints_overwritez.TestSystem.test_velocity_constraints_overwritez  s?     ,7(!!*C!D{{//2h>>>r%   c                    t         d   t         d   z
  | j                  _        | j                  d       | j                  j                  d d  t         d   t         d   z
  gk(  sJ d | j                  _        | j                          y )NrD   rB   r   r   )rG   r    rQ   ra   rz   s     r#   &test_velocity_constraints_back_to_autoz1TestSystem.test_velocity_constraints_back_to_auto  so    +-a52a5=(!!*C!D{{//2r!ur!u}oEEE+/(!!#r%   c                     t        d      t        d      }}t        d      t        d      c} j                  j                  ||        j                  j                  g  j                  ||k(  sJ  j                  j                          j                  j                  g  j                  ||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         j                  j                  fk(  sJ y # 1 sw Y   &xY w)	Nrb1rb2p1p2r5   symbc                  :     j                   j                        S rv   r    ri   )r"   r   s   r#   <lambda>z(TestSystem.test_bodies.<locals>.<lambda>  s    )?)?)E r%   c                  :    j                   j                         S rv   r   )r   r"   s   r#   r   z(TestSystem.test_bodies.<locals>.<lambda>  s    $++*@*@*D r%   )
r   r   r    ri   r2   r   rq   r   r   r   )r"   rY   r   r   r   r   r   s   `    @@r#   test_bodieszTestSystem.test_bodies  so   U#Yu%5S$$BsB'{{!!%<t{{%<C%<%<<<<r"{{!!%@t{{%@C%@%@R%@@@@{{!!R'''{{!!bU***vi!EFj"DE]]9% 	:"%sBD!9DKK	:{{!!bU***	: 	:s   ;F77G c                    t               t        d      t        d      c}t        d      }t        d      }t	        d|      }j                  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        j                  ||j                  ffk(  sJ y # 1 sw Y   (xY w)Nr   r   r   )r   mc1r   c                  T    j                    j                   j                  f      S rv   )r   rl   y)r   r    s   r#   r   z+TestSystem.test_add_loads.<locals>.<lambda>  s     &*:*:AqssACC=*I r%   )r   r   r   r   r   r   r   rl   r
   r1   rq   r   r   r   )r"   r   r   r   r   r   r    s        @@r#   test_add_loadszTestSystem.test_add_loads  s(   c"N3$71Q'EldC QSS)C:uR~F||ACC3*sACCjAAAAACCz||ACC{***j"IJ]]9% 	$qss8FL	$||ACC{***	$ 	$s   0E++E4c                 b   t               }t        d      t        d      }}t        t        d      |j                  |      }t        t        d      |j
                  ||      }|j                  |       |j                  |fk(  sJ |j                  dk(  sJ |f|_        |j                  |fk(  sJ y )Nr   r   T1T2r5   )	r   r   r   r   rl   r   r   r4   r1   )r"   r    r   r   act1act2s         r#   test_add_actuatorszTestSystem.test_add_actuators  s    c"N3$71gdmQSS!4gdmQSS!Q7T"D7***||r!!! 7D7***r%   c           	         t        d      \  }}}}}}}t        dt              \  }}	}
}t        d|||      }t	        d||	||      t        d|	|
||      }t        d|
|||j                  t                    }t               j                  |       j                  |fk(  sJ j                  |fk(  sJ j                  t        |g      k(  sJ j                  t        |g      k(  sJ j                  t        ||j                  t              z
  g      k(  sJ j                  |
       j!                  |       j#                  ||j                  t              z
         j                  |       j                  ||fk(  sJ j                  ||
|	fk(  sJ j                  t        ||g      k(  sJ j                  t        ||g      k(  sJ j                  t        ||j                  t              z
  ||j                  t              z
  g      k(  sJ j#                  ||j                  t              z
          j                         j                  ||fk(  sJ j                  ||
|	fk(  sJ j                  t        |||g      k(  sJ j                  t        |||g      k(  sJ j                  t        ||j                  t              z
  ||j                  t              z
  ||j                  t              z
   g      k(  sJ j                  |       j                  |||fk(  sJ j                  ||
|	|fk(  sJ j                  t        ||||g      k(  sJ j                  t        ||||j                  t              g      k(  sJ j                  t        ||j                  t              z
  ||j                  t              z
  ||j                  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	       y )
Nz	q1:5 u1:4zrb1:6r>   r@   rA   rC   J_lagc                  &    j                         S rv   rH   )rA   r    s   r#   r   z,TestSystem.test_add_joints.<locals>.<lambda>  s    &*;*;B*? r%   c                  &    j                         S rv   r   )r   r    s   r#   r   z,TestSystem.test_add_joints.<locals>.<lambda>  s    ):):3)? r%   )r   r   r   r   r   rg   rh   r   rH   r3   r2   r'   r	   r*   r.   ri   rI   rK   r(   r+   rq   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S#sCdCb"-D#sB3dCb"-#sB
;"}}%%%}}c
***||t4444||t4444{{orBGGAJ.?@@@@#r"RWWQZ("}}R(((}}c3 4444||Bx8888||Bx8888{{o"''!*_b2771:o.0 0 	0 0"rwwqz/*+"}}R,,,}}c3 4444||B|<<<<||B|<<<<{{orBGGAJRWWQZ13bggaj/A/C D D 	D D% }}RU 3333}}c3S 9999||BB/?@@@@||BBGGAJ/GHHHH{{orBGGAJRWWQZ13bggaj/A/C D D 	D D||A"$$$||A"$$$j"?@i!?@r%   c                 N   | j                   dd  | j                  _         | j                  j                   | j                   dd  k(  sJ | j                  d       d| j                  _        d| j                  _        | j                   | j                  _         | j                          y )Nr   )r3   r   r5   )r3   r    ra   r'   r*   rz   s     r#   test_joints_setterzTestSystem.test_joints_setter  s    ![[_{{!!T[[_444!!+!6![[!!#r%   zname, joint_index))r@   r   )rA   r   not_existingNc                 p    | j                   j                  |      }||J y || j                  |   k(  sJ y rv   )r    	get_jointr3   )r"   rY   r   joint_indexjoints        r#   test_get_jointzTestSystem.test_get_joint  s?     %%d+= =DKK4444r%   zname, body_index))r   r   )r   rB   r   c                 p    | j                   j                  |      }||J y || j                  |   k(  sJ y rv   )r    get_bodyr2   )r"   rY   r   
body_indexr   s        r#   test_get_bodyzTestSystem.test_get_body  s?     {{##D)<<4;;z2222r%   r6   c                      G d d|      }| j                   j                  |       t        | j                   j                  |      sJ y )Nc                       e Zd Zy):TestSystem.test_form_eoms_calls_subclass.<locals>.MyMethodN)rn   ro   rp   r5   r%   r#   MyMethodr    s    r%   r  )r6   )r    	form_eomsr   r6   )r"   rm   r6   r  s       r#   test_form_eoms_calls_subclassz(TestSystem.test_form_eoms_calls_subclass  s?    	z 	 	2$++00(;;;r%   zkwargs, expectedrd   explicit_kinematicsc                 r     | j                   j                  di | | j                   j                  |k(  sJ y )Nr5   )r    r  mass_matrix_full)r"   rm   r   r   s       r#   !test_system_kane_form_eoms_kwargsz,TestSystem.test_system_kane_form_eoms_kwargs  s3     	''{{++x777r%   zkwargs, mm, gmc                      | j                   j                  ddt        i| | j                   j                  |k(  sJ | j                   j                  |k(  sJ y )Nr6   r5   )r    r  r   r  forcing_full)r"   rm   r   mmgms        r#   %test_system_lagrange_form_eoms_kwargsz0TestSystem.test_system_lagrange_form_eoms_kwargs  sP     	CCFC{{++r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                  | j                  j                  t         d   | j                  j                  z         t        j                  |      5   | j                  j                  dd|i| d d d        y # 1 sw Y   y xY w)Nr   rc   rd   re   r6   r5   )r)   r    r'   r   r   ri   rj   rk   r   rl   rq   r   r  )r"   r$   r6   r   r   rc   s         r#   test_form_eoms_kwargs_errorsz'TestSystem.test_form_eoms_kwargs_errors!  s     aDSws|,q!	T[[44adT[[]]6JK]]5! 	C!DKK!!BZB6B	C 	C 	Cs   -CC)8rn   ro   rp   rx   r{   rq   markparametrizer   r   r   r   r   r)   r   r,   r   r   r   rM   r   r   rG   r   r
   rl   r   r   r   r   r   r]   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r  r   r   r	  r	   r  rg   rh   r  r  ).0r_   r`   s   000r#   rt   rt   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*<==wr3"s(=>
A$A,!r!u	%r"%aeRV"4	5	5wr3"s(	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&rt   c                   0   e Zd Zej                  j                  deedfeedfg      d        Zej                  j                  dedfedfg      d        Z	d Z
d Zd	 Zej                  j                  dedfedfg      d
        Zd Zd Zd Zd Zd Zy)TestValidateSystemz)valid_method, invalid_method, with_speedsTFc                     | j                  |       | j                  j                  |       t        j                  t
              5  | j                  j                  |       d d d        y # 1 sw Y   y xY w)NrW   )rU   r    rw   rq   r   r   )r"   valid_methodinvalid_methodrT   s       r#   test_only_validz"TestValidateSystem.test_only_valid4  sZ    
 	""{";##L1]]:& 	8KK''7	8 	8 	8s   A,,A5zmethod, with_speedsc                      j                  |        j                  dd   j                  _         j                  d d  j                  _         j                  d d  j                  _        t        j                  t         fd       y )NrW   r   r
  c                  :    j                   j                         S rv   r    rw   methodr"   s   r#   r   zBTestValidateSystem.test_missing_joint_coordinate.<locals>.<lambda>F      $++*E*Ef*M r%   )rU   r'   r    r*   r.   rq   r   r   r"   r&  rT   s   `` r#   test_missing_joint_coordinatez0TestValidateSystem.test_missing_joint_coordinate?  sg     	""{"; JJqrN JJsO99Sb>j"MNr%   c                       j                   d d  j                  _          j                  dd   j                  _         j                  d d  j                  _        t	        j
                  t         fd       y )Nr
  r   c                  8     j                   j                         S rv   r$  r!   s   r#   r   z=TestValidateSystem.test_missing_joint_speed.<locals>.<lambda>L      $++*E*E*G r%   )r'   r    r*   r.   rq   r   r   rz   s   ` r#   test_missing_joint_speedz+TestValidateSystem.test_missing_joint_speedH  sT     JJsO JJqrN99Sb>j"GHr%   c                 |      j                   dd   j                  _         t        j                  t         fd       y )Nr   c                  8     j                   j                         S rv   r$  r!   s   r#   r   z<TestValidateSystem.test_missing_joint_kdes.<locals>.<lambda>P  r,  r%   )r.   r    rq   r   r   rz   s   ` r#   test_missing_joint_kdesz*TestValidateSystem.test_missing_joint_kdesN  s(    99QR=j"GHr%   c                     | j                   d    g| j                   dd  z   | j                  _         | j                  j                          y )Nr   r   )r.   r    rw   rz   s     r#   test_negative_joint_kdesz+TestValidateSystem.test_negative_joint_kdesR  s9    !YYq\M?TYYqr]:##%r%   c                      j                  |       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                  j                         y )NrW   r   r   c                  :    j                   j                         S rv   r$  r%  s   r#   r   zFTestValidateSystem.test_missing_holonomic_constraint.<locals>.<lambda>]  r'  r%   )rU   r    r/   rS   r*   r+   r0   rq   r   r   r(   r'   rw   r(  s   `` r#   !test_missing_holonomic_constraintz4TestValidateSystem.test_missing_holonomic_constraintV  s     	""{";,.)/3xxJJqMDJJqM)DJJqM9;; 0;,j"MN JJ3##F+r%   c                 *    g  j                   _        t        j                  t         fd        j
                  d    j                   _         j                   j
                  d   gz    j                   _         j                   j                          y )Nc                  8     j                   j                         S rv   r$  r!   s   r#   r   zITestValidateSystem.test_missing_nonholonomic_constraint.<locals>.<lambda>d  r,  r%   r   r   )r    r0   rq   r   r   r+   r*   rw   rz   s   ` r#   $test_missing_nonholonomic_constraintz7TestValidateSystem.test_missing_nonholonomic_constraintb  sa    /1,j"GH JJqM JJ$**Q-8##%r%   c                      j                   t        d   gz    j                  _          j                  t        d   t        d   z
  gz    j                  _         j                  j                           j                   j                  _         j                   d d  j                  _          j                  d d  j                  _        t        j                  t         fd       y )Nr   r
  c                  8     j                   j                         S rv   r$  r!   s   r#   r   zFTestValidateSystem.test_number_of_coordinates_speeds.<locals>.<lambda>r  r,  r%   )
r*   r,   r    r.   rG   rw   r'   rq   r   r   rz   s   ` r#   !test_number_of_coordinates_speedsz4TestValidateSystem.test_number_of_coordinates_speedsi  s     JJ!A$/99!r!u~5##% JJ JJsO99Sb>j"G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       y )Nr
  c                  8     j                   j                         S rv   r$  r!   s   r#   r   z8TestValidateSystem.test_number_of_kdes.<locals>.<lambda>w  r,  r%   rB   r   c                  8     j                   j                         S rv   r$  r!   s   r#   r   z8TestValidateSystem.test_number_of_kdes.<locals>.<lambda>y  r,  r%   )r.   r    rq   r   r   r,   rG   rz   s   ` r#   test_number_of_kdesz&TestValidateSystem.test_number_of_kdest  sd    99Sb>j"GH99!qtbe(;'<<j"GHr%   c                 <    | j                   j                  d       y )NT)check_duplicatesr$  rz   s     r#   test_duplicatesz"TestValidateSystem.test_duplicates{  s    ##T#:r%   c           	         t         d t        | j                         | j                  _        t	        j
                  t              5  | j                  j                  t               d d d        g | j                  _        | j                  j                  t               t        | j                  _
        t	        j
                  t              5  | j                  j                  t               d d d        g | j                  _
        | j                  j                  t               | j                  j                  t        dt        d      t        d                   g | j                  _        t	        j
                  t              5  | j                  j                  t               d d d        y # 1 sw Y   KxY w# 1 sw Y   xY w# 1 sw Y   y xY w)NJurbu1rbu2)r,   r   r*   r    rq   r   r   rw   r   rM   r-   rH   r   r   )r"   r[   s     r#   test_speeds_in_lagrangez*TestValidateSystem.test_speeds_in_lagrange  s9   .s4::/]]:& 	9KK''8	9##O4]]:& 	9KK''8	9##O4T9V,i.?@	B]]:& 	9KK''8	9 	9	9 	9
	9 	9	9 	9s$    F. F; G.F8;GGN)rn   ro   rp   rq   r  r  r   r   r!  r)  r-  r0  r2  r5  r8  r;  r?  rB  rG  r5   r%   r#   r  r  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%   r  c                       e Zd Zd Zd Zd Zy)TestSystemExamplesc                 V   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!                  ||       |j"                  ||fk(  sJ |j%                  d      |k(  sJ |j'                  d      |k(  sJ |j)                  | |j                  z         |j+                  |j                  ||j                  z  f       |j-                  t/        ||z  |j                  ||             |j1                          |j3                          t5        |j6                  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;        |jD                  t?        ||z  |	dz  z  tG        |      z  |z   g| |z  |z  tG        |      z  ||z  z   gg      z
        tC        dd      k(  sJ |jI                  tK        |j                  jM                  |j                        jO                  |j                                     |j6                  J ||c|_(        |_)        |	|
c|_*        |_+        |j1                          || tG        |      z  |
| tA        |      z  |	z  |
jY                  tZ              ||	dz  tG        |      z  |	jY                  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_        |j3                         ja                  |      |	jY                  tZ                    jc                               d   }t;        ||z
        dk(  sJ t5        |j6                  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      }|je                  tC        dd            jg                  tC        dd      je                  |            }|jg                  |      }t;        |j<                  |z
        tC        dd      k(  sJ t;        |jD                  |z
        tC        dd      k(  sJ t;        |jh                  |z
        tC        dd      k(  sJ t;        |jj                  |z
        tC        dd      k(  sJ y )Ng l mc mp kzF qp qc up ucrailcartre   bob	bob_frameslider
joint_axispinrR  child_interframechild_pointrB   r   r   rF   )6r   r   r   r   r   r   r   r2   r   r   rj   r   rl   r   zr   rH   r3   r   r  apply_uniform_gravityr   r   r   rw   r  r   r6   r   r   mass_matrixr	   r   r   forcingr   rN   r   pos_fromdotr'   r(   r*   r+   rg   rh   r^   r   xreplacevaluesrow_joincol_joinr  r  )r"   glmcmpkFqpqcupucrL  rM  rN  rO  r    rP  rS  subsupd_expectedupd_solMkgkMdgdMmr  s                              r#   test_cart_pendulum_kanesz+TestSystemExamples.test_cart_pendulum_kanes  s    "-01b"a*?;2r2r b)u2&";/	&&t,}}'''||tzz)))!!T__444$b"PudCRDFF(1q9;;P&#&}}---)V333u%,,,$$aR&((]3$//1tvv:67^AFDFFItLM &++[999++o2grAvB'(26CG+;R!q&[*IJ/L L MA; 	   /!VbAgB'!+,S1Wq[3r7"QV+,;. +/ / 038A;? 	@ ? 	((CNN++DOO<@@JK	M  (((%'"fl%'"fl  QBRLQBRL2%
Aq3r7!2RWWQZ#b'5I!IJL R"Ws2wR!+a"fs1r6{.BR1W.Lq.PP2v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 1v1v.//r2h''Qw|c"gl:Q!Vb[H 2vB/!rBw-#b'2IIK 3r7{A.0 1 b1frkCG#a"f,qAv{R1W/Ds2w/N"g0 AB( )+,rQw;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                    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!                  ||       |j"                  ||fk(  sJ |j%                  d
      |k(  sJ |j'                  d      |k(  sJ |j                  D ]U  }|j(                  |z  |j                  j+                  |j                        j-                  |j                        z  |_        W |j1                  |j                  ||j                  z  f       |j3                  t5        ||z  |j                  ||             |j7                  t8               |j;                  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=        |jF                  tA        ||z  |	dz  z  tI        |      z  |z   g| |z  |z  tI        |      z  ||z  z   gg      z
        tE        dd      k(  sJ |jK                  tM        |j                  j+                  |j                        j-                  |j                                     |jN                  J ||c|_(        |_)        || tI        |      z  |
| tC        |      z  |	z  |
jU                  tV              ||	dz  tI        |      z  |	jU                  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  }|j;                  t8              }|jN                  jX                  d   }|jN                  j[                         |   }t]        |d   j_                  ||i      j_                  |      |	jU                  tV                    d   }t=        ||z
        dk(  sJ ta        |jN                  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      je                  tE        dd            jg                  tE        dd      je                  |jg                  tA        |tC        |      z  ddg      jh                                    }tA        |	|
g|d d  z   |tI        |      z  |	dz  z  gz         }t=        |j>                  |z
        tE        dd      k(  sJ t=        |jF                  |z
        tE        dd      k(  sJ t=        |jj                  |z
        tE        dd      k(  sJ t=        |jl                  |z
        tE        dd      k(  sJ y )NrK  zF qp qczqp qcr   rL  rM  re   rN  rO  rP  rQ  rS  rT  rB   r   r
  rD   r   )7r   r   r   r   r   r   r   r2   r   r   rj   r   rl   r   rW  r   rH   r3   r   r  rf   r[  r\  potential_energyr   r   r   rw   r   r  r   rY  r	   r   r   rZ  r   rN   r   r6   r'   r(   rg   rh   lam_vecsolve_multipliersr   r]  r   r   r_  r`  r   r  r  )r"   ra  rb  rc  rd  re  rf  rg  rh  qpdqcdrL  rM  rN  rO  r    rP  rS  r   rk  qpdd_expectedeomslam1lam1_solqpdd_solrp  rq  rr  r  s                                r#   test_cart_pendulum_lagrangez.TestSystemExamples.test_cart_pendulum_lagrange  s    "-01b"a"9-	2r!'1-S b)u2&";/	&&t,}}'''||tzz)))!!T__444$b#+/663udCSTVV(1q9;;P&#&}}---)V333u%,,,MM 	2D$(IIMDOO4L4L""5$$'CM%2D!	2 	$//1tvv:67^AFDFFItLM/)++o2grAvB'(26CG+;R!q&[*IJ/L L MA; 	   /!VcQhR(1,-a!c"g0EB0N/O;Q +
 
 Aqk 	  	((CNN++DOO<@@JK	M  (((%'"fl QBRLaR#b'\C'Q#(SW"4sxx{SW7L"LMO R"Ws2wR!+a"fs1r6{.BSAX.M/ R#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{AFSW,<qb3r7lK 2vB/b"=? @b1frkCG#a"f,-"fs2w)A-.01 !fooeAqk*44U1a[5I5IKKSWa(;<>>?6A Bc3Z"Q%/1s2w;3I2JJK**R/0E!QK???+,a;;;//"45q!DDD++b01U1a[@@@r%   c           	          t        d      \  }}}t        d      \  }}}t        dd      \  }}t        d|      }	t               }
|
j	                  |	       |	j
                  j                  |
j                  ||
j                  z         |	j
                  j                  |
j                  ||
j                  z  ||
j                  z  z          |g|g|gc|
_        |
_        |
_        |j                  t               |z
  g|
_        |
j%                  | |
j                  z         |
j'                  t)        |	||
j                  z        t)        |	||
j                  z  ||z  |
j                  z  z
               |
j+                          |
j-                          t/        dg      }t/        |g      }t/        |g      }t/        |||z  z
  g      }|j1                  t3        dd            j5                  t3        dd      j1                  |            }|j5                  |      }t/        |||z  z
  g      }t7        |
j8                  |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@                  jB                  |z
        t3        dd      k(  sJ y )
Nzg m muzq u uazN FT)positiver   re   r   rB   )"r   r   r   r   ri   rj   rk   r   rl   set_velr   r   r'   r*   r-   rg   rh   r.   rX  r   r
   rw   r  r	   r_  r   r`  r   rY  rZ  r  r  r6   auxiliary_eqs)r"   ra  rd   mur)   r,   rM   r   rf  r   r    rn  ro  rp  rq  rr  r  aux_eqss                     r#   test_box_on_groundz%TestSystemExamples.test_box_on_ground  s    8$1b!(+1bed31Sq!!	V//VXX>	V\\1vxx<"vxx-+GH453bT0flFLvvay1}o$$aR&((]3!Q\"!Q\BFVXX$556	8 	  aS!aS!aS!a"q&j\*kk%1+&00q!1E1Eb1IJ[[_!1q1u9+.**R/0E!QK???+,a;;;//"45q!DDD++b01U1a[@@@))77'A "1a[) 	) )r%   N)rn   ro   rp   rs  r  r  r5   r%   r#   rI  rI    s    CAJEAN!)r%   rI  ))rq   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   _trh   r)   rG   r,   rM   r   rt   r  rI  r5   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%   