
    sg                     h    d dl mc mc mZ d dlmZmZmZmZm	Z	 d dl
mZ d dlmZ d Zd Zd Zd Zy)	    N)cossinMatrixsymbolszeros)simplify)dynamicsymbolsc                     t        d      \  } }}t        d      }t        d      \  }}}t        j                  d      }t	        |gg      }t	        |  |z  ||z  z
  gg      }	t        ||j                  z
        t	        dg      k(  sJ t        |	|j                  z
        t	        dg      k(  sJ t        j                  dd      }
t	        |gg      }t	        |  |z  ||z  z   ||z  z
  gg      }t        ||
j                  z
        t	        dg      k(  sJ t        ||
j                  z
        t	        dg      k(  sJ t        j                  ddd      }t	        |gg      }t	        |  |z  ||z  z   ||z  z
  |z   gg      }t        ||j                  z
        t	        dg      k(  sJ t        ||j                  z
        t	        dg      k(  sJ t        j                  ddd      }t	        |gg      }t	        |  |z  ||z  z
  |z   gg      }t        ||j                  z
        t	        dg      k(  sJ t        ||j                  z
        t	        dg      k(  sJ y )Nc0 k0 m0gzv0 x0 f0   r   TF)r   r	   modelsmulti_mass_spring_damperr   r   mass_matrixforcing)c0k0m0r   v0x0f0kane1massmatrix1forcing1kane2massmatrix2forcing2kane3massmatrix3forcing3kane4massmatrix4forcing4s                      \/var/www/html/venv/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_models.py$test_multi_mass_spring_damper_inputsr%      sU   $JBBA
+JBB++A.E2$.KBB'()HK%"3"334sCCCHu}},-<<<++At4E2$.KB22-./0HK%"3"334sCCCHu}},-<<<++AtT:E2$.KB22-2345HK%"3"334sCCCHu}},-<<<++Aud;E2$.KBB+,-.HK%"3"334sCCCHu}},-<<<    c                  "   t        d      \  } }}t        d      \  }}}t        d      \  }}}t        d      \  }	}
t        d      \  }}t        d      \  }}t        j                  d      }t	        ||z   |z   ||z   |g||z   ||z   |g|||gg      }t	        |  |	z  ||
z  z
  g| |z  ||z  z
  g| |z  ||z  z
  gg      }t        ||j                  z
        t        d      k(  sJ t        ||j                  z
        t	        g d      k(  sJ y )	Nr   zc1 k1 m1zc2 k2 m2zv0 x0zv1 x1zv2 x2   r   r   r   )	r   r	   r   r   r   r   r   r   r   )r   r   r   c1k1m1c2k2m2r   r   v1x1v2x2r   r   r   s                     r$   *test_multi_mass_spring_damper_higher_orderr4   &   s<   $JBB$JBB$JBBG$FBG$FBG$FB++A.E27R<b"57BGR0r2,( )K BB'BB'BB') *H K%"3"334a@@@Hu}},-	1BBBBr&   c                  D   t        d      \  } }t        d      }t        d      }t        d      \  }}}}t        d      \  }}	t        j                  d      }
t	        ||z   |  |z  t        |      z  g|  |z  t        |      z  | dz  |z  gg      }t	        |  |z  |	dz  z  t        |      z  |z   g|| z  |z  t        |      z  gg      }t        ||
j                  z
        t        d      k(  sJ t        ||
j                  z
        t	        ddg      k(  sJ t        j                  dd	      }t	        ||z   |  |z  t        |      z  g|  |z  t        |      z  | dz  |z  gg      }t	        |  |z  |	dz  z  t        |      z  g|| z  |z  t        |      z  gg      }t        ||j                  z
        t        d      k(  sJ t        ||j                  z
        t	        ddg      k(  sJ t        j                  dd	d
      }t	        ||z   |  |z  t        |      z  g|  |z  t        |      z  | dz  |z  gg      }t	        |  |z  |	dz  z  t        |      z  g|| z  |z  t        |      z  |z   gg      }t        ||j                  z
        t        d      k(  sJ t        ||j                  z
        t	        ddg      k(  sJ t        j                  dd
d	      }t	        ||z   |  |z  t        |      z  g|  |z  t        |      z  | dz  |z  gg      }t	        |  |z  |	dz  z  t        |      z  |z   g|| z  |z  t        |      z  gg      }t        ||j                  z
        t        d      k(  sJ t        ||j                  z
        t	        ddg      k(  sJ y )Nl0 m0r,   r   z
q0 q1 F T1zu0 u1r      r   FTr   r	   r   n_link_pendulum_on_cartr   r   r   r   r   r   r   )l0r   r,   r   q0q1FT1u0u1r   r   r   r   r   r   r   r   r    r!   r"   r#   s                         r$   #test_n_link_pendulum_on_cart_inputsrA   9   s   WFB	BA"<0MBArG$FB**1-E27RCF3r7N33r6#b'>2q5846 7KBr1uSW,q01AbDGCGO3DEFHK%"3"334a@@@Hu}},-A???**1e4E27RCF3r7N33r6#b'>2q5846 7KBr1uSW,-"RB/@ABHK%"3"334a@@@Hu}},-A???**1eT:E27RCF3r7N33r6#b'>2q5846 7KBr1uSW,-"RB"0D/EFGHK%"3"334a@@@Hu}},-A???**1dE:E27RCF3r7N33r6#b'>2q5846 7KBr1uSW,q01AbDGCGO3DEFHK%"3"334a@@@Hu}},-A???r&   c                     t        d      \  } }t        d      \  }}t        d      }t        d      }t        d      \  }}}t        d      \  }	}
}t        d      \  }}t        j                  d      }t	        ||z   |z   |  |z  t        |      z  | |z  t        |      z  z
  | |z  t        |      z  g|  |z  t        |      z  | |z  t        |      z  z
  | dz  |z  | dz  |z  z   | |z  |z  t        |      t        |      z  t        |      t        |      z  z   z  g| |z  t        |      z  | |z  |z  t        |      t        |      z  t        |      t        |      z  z   z  |dz  |z  gg      }t	        |  |z  |
dz  z  t        |      z  | |z  |
dz  z  t        |      z  z
  ||z  |dz  z  t        |      z  z
  |z   g|| z  |z  t        |      z  || z  |z  t        |      z  z   | |z  |z  t        |      t        |      z  t        |      t        |      z  z
  z  |dz  z  z
  g||z  |z  t        |      z  | |z  |z  t        |       t        |      z  t        |      t        |      z  z   z  |
dz  z  z
  gg      }t        ||j                  z
        t        d	      k(  sJ t        ||j                  z
        t	        g d
      k(  sJ y )Nr6   zl1 m1r/   r   zq0 q1 q2zu0 u1 u2zF T1r7   r(   r)   r8   )r:   r   l1r,   r/   r   r;   r<   q2r?   r@   u2r=   r>   r   r   r   s                    r$   )test_n_link_pendulum_on_cart_higher_orderrF   ]   s   WFBWFB	BA
+JBB
+JBB6"EAr**1-E27R<"RB"R%B-)G3r6#b'>+3r6#b'>BrE#b'M92q58b!eBh;Nb58SWSW_s2ws2w%FGI3r6#b'>b58SWSW_s2ws2w%FGq58%	& 'K Br1uSW,r"uRU{3r7/BB2b!eCG+,./0 1"RB!B$r'#b'/92b#b'#b'/CGCGO"CDRUJK L"RB"R%(SWHSW4D47GCGO5D +EEGU+K K L	M NH K%"3"334a@@@Hu}},-	1BBBBr&   )sympy.physics.mechanics.modelsphysics	mechanicsr   sympyr   r   r   r   r   sympy.simplify.simplifyr   sympy.physics.mechanicsr	   r%   r4   rA   rF    r&   r$   <module>rN      s/    / / 4 4 , 4=>C&!@HCr&   