o
    ŗ„gÖ  ć                   @   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Śx0Śf0Śkane1Śmassmatrix1Śforcing1Śkane2Śmassmatrix2Śforcing2Śkane3Śmassmatrix3Śforcing3Śkane4Śmassmatrix4Śforcing4© r"   ś`/var/www/visachat/venv/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sJ 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   Śc1Śk1Śm1Śc2Śk2Śm2r   r   Śv1Śx1Śv2Śx2r   r   r   r"   r"   r#   Ś*test_multi_mass_spring_damper_higher_order&   s"   
žž"r1   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   Ś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’: ’:$r=   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 )Nr2   zl1 m1r,   r
   zq0 q1 q2zu0 u1 u2zF T1r3   r%   r&   r4   )r6   r   Śl1r)   r,   r
   r7   r8   Śq2r;   r<   Śu2r9   r:   r   r   r   r"   r"   r#   Ś)test_n_link_pendulum_on_cart_higher_order]   sF   
,’6*’*
žü2’’&2’,’’ü$rA   )Śsympy.physics.mechanics.modelsŚphysicsŚ	mechanicsr   Śsympyr   r   r   r   r   Śsympy.simplify.simplifyr   Śsympy.physics.mechanicsr   r$   r1   r=   rA   r"   r"   r"   r#   Ś<module>   s    $