o
    GZh!                     @   s.  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 gd de e gd d ed e ggZed d ed ed  ee  gZeg d	g d
d d dd e e gd d d de e gd d d d ed e ggZeeed d ed ed  ee  gZeeegZeeZeee ZedZe ddeej!gZ"e
dZ#e#$dee"j Z%e	de%eZ&e&gZ'e%ee ej fgZ(edZ)e)ee ee  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d Z2dd Z3dd Z4dd Z5dd 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AZAxisOPPaPE   )r   r   )r      c               
   C   s0  t ttttttttd} | 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s0J | jdgks8J t}t| j| tddksHJ t|  ttttthksWJ t|  tksaJ t|  ttthksnJ t|  tksxJ | jtksJ | jtfksJ | jttt tj ffksJ 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inter r4   X/var/www/auris/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_system.pytest_form_1C   s$   "r6   c               
   C   st  t ttttttttd} | 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s1J | jdgks9J t}t| j| tddksIJ t| jt tdksVJ t|  ttttt
hkseJ t|  tksoJ t|  ttthks|J t|  tksJ t}|   t| j| tddksJ | jtksJ | jtfksJ | jttt t j
 ffksJ 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,   r-   r.   r/   r0   r1   r    compute_explicit_formr   r   r   r   )
symsystem2r3   r4   r4   r5   test_form_2]   s,   "r<   c                  C   s  t ttttttttt	d	} | 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s1J | jdgks9J t}t}t| j| tddksKJ t| jt tdksXJ t| j| tddksfJ t}t| j| tddksvJ t| jt tdksJ t}|   t| j| tddksJ t|  ttttthksJ t|  tksJ t|  ttthksJ t|  tksJ | j i ksJ | jt!fksJ | j	t"tt t#j ffksJ d S )N)r7   Z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   r8   r9   r    r:   r*   r+   r,   r-   r.   r/   r0   r1   r   r   r   r   )Z
symsystem3Zinter1Zinter2r3   r4   r4   r5   test_form_3}   s:   "r@   c               
   C   sJ  t ttttttttd} t	t
 d| _W d    n1 sw   Y  t	t
 d| _W d    n1 s5w   Y  t	t
 d| _W d    n1 sLw   Y  t	t
 d| _W d    n1 scw   Y  t	t
 d| _W d    n1 szw   Y  t	t
 d| _W d    n1 sw   Y  t	t
 d| _W d    n1 sw   Y  t	t
 d| _W d    n1 sw   Y  t	t
 d| _W d    n1 sw   Y  t	t
 d| _W d    n1 sw   Y  t	t
 d| _W d    n	1 sw   Y  t	t
 d| _W d    d S 1 sw   Y  d S )Nr   *   )r   r   r    r!   r"   r   r   r   r   r   AttributeErrorr#   r=   r8   r>   r9   r?   r&   r   )Z	symsystemr4   r4   r5   test_property_attributes   sR   











$rC   c                  C   s  t tt} tt | j W d   n1 sw   Y  tt | j W d   n1 s.w   Y  tt | j W d   n1 sEw   Y  tt | j W d   n1 s\w   Y  tt | j	 W d   n1 ssw   Y  tt | 
  W d   n1 sw   Y  t ttttd}tt |j W d   n1 sw   Y  tt |j W d   n1 sw   Y  tt |j	 W d   n1 sw   Y  tt | j W d   n1 sw   Y  tt | j W d   n	1 sw   Y  tt | j W d   n	1 sw   Y  tt | j W d   n	1 s7w   Y  tt |j W d   dS 1 sPw   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&   r7   )r   r   r    r   rB   r9   r8   r>   r=   r?   r:   r#   r&   r   r   )r2   r;   r4   r4   r5   test_not_specified_errors   s\   















$rD   N)7Zsympyr   r   r   r   Zsympy.simplify.simplifyr   Zsympy.physics.mechanicsr   r   r	   r
   r   Zsympy.testing.pytestr   r$   r%   r'   r(   r)   r1   r/   r0   r>   r=   r9   r8   r?   ZLUsolver    thetar   Z	orientnewzr   r   Z	locatenewr   r   r   r   r   r"   r   r!   r#   r&   r   r   r   r6   r<   r@   rC   rD   r4   r4   r4   r5   <module>   sT    "&

 ' 