
    g!                     |   d dl mZmZmZmZ d dlmZ d dlmZm	Z	m
Z
mZmZ d dlmZ  ed          \  ZZZZZ ed          \  ZZZ edd e ez  gd de ez  gd d edz  ez  gg          Z ed d edz  edz  z   eez  z
  g          Z eg d	g d
d d dd e ez  gd d d de ez  gd d d d edz  ez  gg          Z eeed d edz  edz  z   eez  z
  g          Z eeeg          Ze                    e          Z eeez            Z ed          Ze                     ddeej!        g          Z" e
d          Z#e#$                    dee"j        z            Z% e	de%e          Z&e&gZ'e%eez  ej        z  fgZ( ed          Z)e)eez  eez   z  iZ*dgZ+dgZ,eeefZ-eefZ.eeeeefZ/dZ0dZ1d Z2d Z3d Z4d Z5d Z6dS )    )symbolsMatrixatanzeros)simplify)dynamicsymbolsParticlePointReferenceFrameSymbolicSystem)raiseszx y u v lambdazm l g      )r   r   r   r   r   )r   r   r   r   r   NAAxisOPPaPE   )r   r   )r      c            
      `   t          t          t          t          t          t
          t          t          t                    } | j	        t          t          t          g          k    sJ | j        t          t          t          g          k    sJ | j        t          t          t          t          t          t           g          k    sJ | j        dgk    sJ t          }t%          | j        |z
            t'          dd          k    sJ t)          |                                           t          t          t           t          t          hk    sJ t-          |                                           t.          k    sJ t)          |                                           t2          t4          t6          hk    sJ t-          |                                           t.          k    sJ | j        t          k    sJ | j        t:          fk    sJ | j        t<          t4          t6          z  t>          j        z  ffk    sJ d S )Nalg_conoutput_eqns
coord_idxs
speed_idxsbodiesloadsr      r   ) r   statescomb_explicit_rhsalg_con_fullout_eqnsr   r   r   r    coordinatesr   xyspeedsuvlamr   r   r   setdynamic_symbolstypetupleconstant_symbolslgmr   r   r   r   )
symsystem1inters     e/var/www/html/ai-engine/env/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_system.pytest_form_1r8   C   s   (9(4(+5*'-U< < <J
 !VQF^^33331v....1aC'8 9 99999!$$$$EJ05899U1a[[HHHHz))++,,AsAq0AAAAA
**,,--6666z**,,--!Q::::
++--..%7777!X----%%%%AEACK 02222222    c            
      V   t          t          t          t          t          t
          t          t          t                    } | j        t          t          t          t          g          k    sJ | j        t          t          t          g          k    sJ | j        t          t          t          t          t          t          g          k    sJ | j        dgk    sJ t          }t#          | j        |z
            t%          dd          k    sJ t#          | j        t          z
            t%          d          k    sJ t'          |                                           t          t          t          t          t          hk    sJ t+          |                                           t,          k    sJ t'          |                                           t0          t2          t4          hk    sJ t+          |                                           t,          k    sJ t6          }|                                  t#          | j        |z
            t%          dd          k    sJ | j        t          k    sJ | j        t<          fk    sJ | j        t>          t2          t4          z  t@          j
        z  ffk    sJ d S )N)r)   mass_matrixr   r   r   r    r   r!   r   )!r   r&   comb_implicit_rhsr)   comb_implicit_matr$   r%   r   r    r   r'   r(   r,   r*   r+   r"   r   r   r   r-   r.   r/   r0   r1   r2   r3   r4   r#   compute_explicit_formr   r   r   r   )
symsystem2r6   s     r7   test_form_2r@   ]   s<   ->v,=(4('-U< < <J
 !VQ3K%8%888881v....1c1a'8 9 99999!$$$$EJ05899U1a[[HHHHJ01BBCCuQxxOOOOz))++,,AsAq0AAAAA
**,,--6666z**,,--!Q::::
++--..%7777E$$&&&J05899U1a[[HHHH !X----%%%%AEACK 02222222r9   c                  r   t          t          t          t          t          t
          t          t          t          t          	  	        } | j
        t          t          t          g          k    sJ | j        t          t          t           g          k    sJ | j        t          t          t          t          t           t"          g          k    sJ | j        dgk    sJ t          }t          }t%          | j        |z
            t'          dd          k    sJ t%          | j        t          z
            t'          d          k    sJ t%          | j        |z
            t'          dd          k    sJ t(          }t%          | j        |z
            t'          dd          k    sJ t%          | j        t*          z
            t'          d          k    sJ t,          }|                                  t%          | j        |z
            t'          dd          k    sJ t1          |                                           t          t           t"          t          t          hk    sJ t5          |                                           t6          k    sJ t1          |                                           t:          t<          t>          hk    sJ t5          |                                           t6          k    sJ | j         i k    sJ | j        tB          fk    sJ | j	        tD          t<          t>          z  tF          j        z  ffk    sJ d S )N)r;   coordinate_derivativesr   r   r   r   r    r   r   r   r   r!   )$r   r"   dyn_implicit_rhsdyn_implicit_matkin_explicit_rhsr   r   r   r   r    r&   r   r'   r(   r)   r*   r+   r,   r   r   r<   r=   r#   r>   r-   r.   r/   r0   r1   r2   r3   r4   r   r   r   r   )
symsystem3inter1inter2r6   s       r7   test_form_3rI   }   s   (8,<7G(/J+5f&+- - -J !VQF^^33331v....1aC'8 9 99999!$$$$FFJ/&899U1a[[HHHHJ/2BBCCuQxxOOOOJ/&899U1a[[HHHHEJ05899U1a[[HHHHJ01BBCCuQxxOOOOE$$&&&J05899U1a[[HHHHz))++,,AsAq0AAAAA
**,,--6666z**,,--!Q::::
++--..%7777!R''''%%%%AEACK 02222222r9   c            
      N   t          t          t          t          t          t
          t          t          t                    } t          t                    5  d| _        d d d            n# 1 swxY w Y   t          t                    5  d| _        d d d            n# 1 swxY w Y   t          t                    5  d| _        d d d            n# 1 swxY w Y   t          t                    5  d| _        d d d            n# 1 swxY w Y   t          t                    5  d| _        d d d            n# 1 swxY w Y   t          t                    5  d| _        d d d            n# 1 swxY w Y   t          t                    5  d| _        d d d            n# 1 swxY w Y   t          t                    5  d| _        d d d            n# 1 swxY w Y   t          t                    5  d| _        d d d            n# 1 swxY w Y   t          t                    5  d| _        d d d            n# 1 swxY w Y   t          t                    5  d| _        d d d            n# 1 swxY w Y   t          t                    5  d| _        d d d            d S # 1 swxY w Y   d S )Nr   *   )r   r"   r#   r$   r%   r   r   r   r    r   AttributeErrorr&   rC   r<   rD   r=   rE   r)   r   )	symsystems    r7   test_property_attributesrN      s>   v'8'3*4&,E; ; ;I
 
		  	              			 # # "	# # # # # # # # # # # # # # #			 ( (%'	"( ( ( ( ( ( ( ( ( ( ( ( ( ( (			 ) )&(	#) ) ) ) ) ) ) ) ) ) ) ) ) ) )			  	              			 ( (%'	"( ( ( ( ( ( ( ( ( ( ( ( ( ( (			 ) )&(	#) ) ) ) ) ) ) ) ) ) ) ) ) ) )			 ( (%'	"( ( ( ( ( ( ( ( ( ( ( ( ( ( (			 ) )&(	#) ) ) ) ) ) ) ) ) ) ) ) ) ) )			  	              			  	              			  	                 s   A((A,/A,BB"B:CCC-DDD D44D8;D8E''E+.E+FF!F9GGG,H  HHH33H7:H7I&&I*-I*JJ!Jc                  8   t          t          t                    } t          t                    5  | j         ddd           n# 1 swxY w Y   t          t                    5  | j         ddd           n# 1 swxY w Y   t          t                    5  | j         ddd           n# 1 swxY w Y   t          t                    5  | j         ddd           n# 1 swxY w Y   t          t                    5  | j	         ddd           n# 1 swxY w Y   t          t                    5  | 
                                 ddd           n# 1 swxY w Y   t          t          t          t          t
                    }t          t                    5  |j         ddd           n# 1 swxY w Y   t          t                    5  |j         ddd           n# 1 swxY w Y   t          t                    5  |j	         ddd           n# 1 swxY w Y   t          t                    5  | j         ddd           n# 1 swxY w Y   t          t                    5  | j         ddd           n# 1 swxY w Y   t          t                    5  | j         ddd           n# 1 swxY w Y   t          t                    5  | j         ddd           n# 1 swxY w Y   t          t                    5  |j         ddd           dS # 1 swxY w Y   dS )zThis test will cover errors that arise from trying to access attributes
    that were not specified upon object creation or were specified on creation
    and the user tries to recalculate them.N)r)   r;   )r   r"   r#   r   rL   r=   r<   rD   rC   rE   r>   r&   r)   r   r    )r5   r?   s     r7   test_not_specified_errorsrP      s<     (9::J			 % %$$% % % % % % % % % % % % % % %			 % %$$% % % % % % % % % % % % % % %			 $ $##$ $ $ $ $ $ $ $ $ $ $ $ $ $ $			 $ $##$ $ $ $ $ $ $ $ $ $ $ $ $ $ $			 $ $##$ $ $ $ $ $ $ $ $ $ $ $ $ $ $			 + +((***+ + + + + + + + + + + + + + +  ->v,=? ? ?J 
		 $ $##$ $ $ $ $ $ $ $ $ $ $ $ $ $ $			 $ $##$ $ $ $ $ $ $ $ $ $ $ $ $ $ $			 $ $##$ $ $ $ $ $ $ $ $ $ $ $ $ $ $
 
		                			                
 
		                			                
 
		 % %$$% % % % % % % % % % % % % % % % % %s   AA
A"A66A:=A:B))B-0B-CC #C ;DDD.EEEF))F-0F-GG #G ;HHH.II	I!I55I9<I9J((J,/J,KK"K:LLLN)7sympyr   r   r   r   sympy.simplify.simplifyr   sympy.physics.mechanicsr   r	   r
   r   r   sympy.testing.pytestr   r'   r(   r*   r+   r,   r4   r2   r3   rD   rC   r=   r<   rE   LUsolver#   thetar   	orientnewzr   r   	locatenewr   r   r   r    r   r%   r   r$   r&   r)   r"   r   r   r8   r@   rI   rN   rP    r9   r7   <module>r[      sP   . . . . . . . . . . . . , , , , , ,E E E E E E E E E E E E E E ' ' ' ' ' ' !.!1221aC
''

1a 6Aq1"Q$<q1"Q$<q!Q$q&>+ , ,  61aA1qs!2344 FOOO+OO1a!A.1a!A.1aAa0	2 3 3  FAq!Q1q!tac(9:;; 61a&>> %--.?@@  	QqS		N3KKVeQS\**	E#JJKKQWXdAq

QUQS[	 WT]]!QqS	? #s!Sk
Q
Q1c	

3 3 343 3 3@$3 $3 $3N  @1% 1% 1% 1% 1%r9   