o
    GZŽhÖ  ã                   @   sp   d dl m  m  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d„ Zdd„ Zd	d
„ Zdd„ ZdS )é    N)ÚcosÚsinÚMatrixÚsymbolsÚzeros)Úsimplify)Údynamicsymbolsc                  C   sê  t dƒ\} }}t dƒ}tdƒ\}}}t d¡}t|ggƒ}t|  | ||  ggƒ}	t||j ƒtdgƒks8J ‚t|	|j ƒtdgƒksFJ ‚t dd¡}
t|ggƒ}t|  | ||  ||  ggƒ}t||
j ƒtdgƒksqJ ‚t||
j ƒtdgƒksJ ‚t ddd¡}t|ggƒ}t|  | ||  ||  | ggƒ}t||j ƒtdgƒks­J ‚t||j ƒtdgƒks»J ‚t ddd¡}t|ggƒ}t|  | ||  | ggƒ}t||j ƒtdgƒksåJ ‚t||j ƒtdgƒksóJ ‚d S )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Úx0Zf0Úkane1Úmassmatrix1Úforcing1Úkane2Úmassmatrix2Úforcing2Úkane3Úmassmatrix3Úforcing3Úkane4Úmassmatrix4Úforcing4© r!   úX/var/www/auris/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_models.pyÚ$test_multi_mass_spring_damper_inputs   s.   
"& r#   c                  C   s  t dƒ\} }}t dƒ\}}}t dƒ\}}}tdƒ\}	}
tdƒ\}}tdƒ\}}t d¡}t|| | || |g|| || |g|||ggƒ}t|  |	 ||
  g| | ||  g| | ||  ggƒ}t||j ƒtdƒksrJ ‚t||j ƒtg d¢ƒksJ ‚d S )	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   Úc1Zk1Úm1Úc2Zk2Úm2r   r   Úv1Úx1Úv2Zx2r   r   r   r!   r!   r"   Ú*test_multi_mass_spring_damper_higher_order&   s"   
þþ"r-   c                  C   s*  t dƒ\} }t dƒ}t dƒ}tdƒ\}}}}tdƒ\}}	t d¡}
t|| |  | t|ƒ g|  | t|ƒ | d | ggƒ}t|  | |	d  t|ƒ | g||  | t|ƒ ggƒ}t||
j ƒt	dƒksiJ ‚t||
j
 ƒtddgƒksxJ ‚t dd	¡}t|| |  | t|ƒ g|  | t|ƒ | d | ggƒ}t|  | |	d  t|ƒ g||  | t|ƒ ggƒ}t||j ƒt	dƒksÄJ ‚t||j
 ƒtddgƒksÓJ ‚t dd	d
¡}t|| |  | t|ƒ g|  | t|ƒ | d | ggƒ}t|  | |	d  t|ƒ g||  | t|ƒ | ggƒ}t||j ƒt	dƒks#J ‚t||j
 ƒtddgƒks3J ‚t dd
d	¡}t|| |  | t|ƒ g|  | t|ƒ | d | ggƒ}t|  | |	d  t|ƒ | g||  | t|ƒ ggƒ}t||j ƒt	dƒksƒJ ‚t||j
 ƒtddgƒks“J ‚d S )Núl0 m0r'   r
   z
q0 q1 F T1zu0 u1r   é   r   FT©r   r   r   Z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    r!   r!   r"   Ú#test_n_link_pendulum_on_cart_inputs9   sB   
ÿ:ÿ6ÿ: ÿ:$r8   c               	   C   s‚  t dƒ\} }t dƒ\}}t dƒ}t dƒ}tdƒ\}}}tdƒ\}	}
}tdƒ\}}t d¡}t|| | |  | t|ƒ | | t|ƒ  | | t|ƒ g|  | t|ƒ | | t|ƒ  | d | | d |  | | | t|ƒt|ƒ t|ƒt|ƒ   g| | t|ƒ | | | t|ƒt|ƒ t|ƒt|ƒ   |d | ggƒ}t|  | |
d  t|ƒ | | |
d  t|ƒ  || |d  t|ƒ  | g||  | t|ƒ ||  | t|ƒ  | | | t|ƒt|ƒ t|ƒt|ƒ   |d   g|| | t|ƒ | | | t|ƒ t|ƒ t|ƒt|ƒ   |
d   ggƒ}t||j ƒt	d	ƒks/J ‚t||j
 ƒtg d
¢ƒks?J ‚d S )Nr.   zl1 m1r)   r
   zq0 q1 q2zu0 u1 u2zF T1r/   r$   r%   r0   )r1   r   Úl1r'   r)   r
   r2   r3   Úq2r6   r7   Úu2r4   r5   r   r   r   r!   r!   r"   Ú)test_n_link_pendulum_on_cart_higher_order]   sF   
,ÿ6*ÿ*
þü2ÿÿ&2ÿ,ÿÿü$r<   )Zsympy.physics.mechanics.modelsZphysicsZ	mechanicsr   Zsympyr   r   r   r   r   Zsympy.simplify.simplifyr   Zsympy.physics.mechanicsr   r#   r-   r8   r<   r!   r!   r!   r"   Ú<module>   s    $