a
    kh!                     @   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   s4  t ttttttttd} | j	t
ttgks.J | jt
ttgksDJ | jt
tttttgks`J | jdgkspJ 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tksJ | jtfksJ | jttt tj ffks0J 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   W/var/www/auris/lib/python3.9/site-packages/sympy/physics/mechanics/tests/test_system.pytest_form_1C   s$    r6   c               
   C   s~  t ttttttttd} | jt	t
ttgks0J | jt	ttgksFJ | jt	t
ttttgksbJ | jdgksrJ t}t| j| tddksJ t| jt t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sJ t}|   t| j| tddks8J | jtksHJ | jtfksZJ | jttt t j
 ffkszJ 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s0J | jtttgksFJ | jttttttgksbJ | jdgksrJ t}t}t| j| tddksJ t| jt tdksJ t| j| tddksJ t}t| j| tddksJ t| jt tdksJ t}|   t| j| tddks2J t|  ttttthksRJ t|  tkshJ 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   sR  t ttttttttd} t	t
 d| _W d    n1 s<0    Y  t	t
 d| _W d    n1 sj0    Y  t	t
 d| _W d    n1 s0    Y  t	t
 d| _W d    n1 s0    Y  t	t
 d| _W d    n1 s0    Y  t	t
 d| _W d    n1 s$0    Y  t	t
 d| _W d    n1 sT0    Y  t	t
 d| _W d    n1 s0    Y  t	t
 d| _W d    n1 s0    Y  t	t
 d| _W d    n1 s0    Y  t	t
 d| _W d    n1 s0    Y  t	t
 d| _W d    n1 sD0    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   s:    
$
$
$
$
$
&
&
&
&
&
&
rC   c                  C   s  t tt} tt | j W d   n1 s.0    Y  tt | j W d   n1 s\0    Y  tt | j W d   n1 s0    Y  tt | j W d   n1 s0    Y  tt | j	 W d   n1 s0    Y  tt | 
  W d   n1 s0    Y  t ttttd}tt |j W d   n1 sX0    Y  tt |j W d   n1 s0    Y  tt |j	 W d   n1 s0    Y  tt | j W d   n1 s0    Y  tt | j W d   n1 s0    Y  tt | j W d   n1 sH0    Y  tt | j W d   n1 sx0    Y  tt |j W d   n1 s0    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>   sR   "&

 ' 