U
    ZÊ¹jò'  ã                   @   sú   d dl mZ d dlmZ d dlmZmZ d dlmZ d dl	m
Z
 d dlmZmZmZmZmZmZmZmZmZ d dlmZmZ d dlmZmZ d d	lmZ d d
lmZ d dlm Z  ej!Z"dd„ Z#dd„ Z$dd„ Z%dd„ Z&dd„ Z'dd„ Z(dd„ Z)dd„ Z*dS )é    )Úexpand)Úsymbols)ÚcosÚsin)ÚMatrix)Útrigsimp)	ÚPinJointÚJointsMethodÚ	RigidBodyÚParticleÚBodyÚKanesMethodÚPrismaticJointÚLagrangesMethodÚinertia)ÚdynamicsymbolsÚReferenceFrame)ÚraisesÚwarns_deprecated_sympy)Úzeros)Úlambdify)Úsolvec            	   	   C   sp  t ƒ � tdƒ} tdƒ}W 5 Q R X td| |ƒ}tdƒ\}}tdƒ\}}|  || j ¡ t ƒ � t| |ƒ}W 5 Q R X |j| jks‚t	‚|j
|| gks”t	‚|j| j|| jj fgks²t	‚|jt|gƒksÆt	‚|jt|gƒksÚt	‚|jt|| ¡  gƒksöt	‚| ¡ }|t| | ¡  ggƒk�st	‚|jt|gdggƒk�s:t	‚|jtddgd|ggƒk�sZt	‚t|jtƒ�slt	‚d S )NÚPÚCÚP1zC_ixx gz
q_P1, u_P1r   é   )r   r   r   r   r   Úapply_forceÚyr	   ÚframeÚAssertionErrorÚbodiesÚloadsÚ
masscenterÚqr   ÚuÚkdesÚdiffÚ	form_eomsÚforcing_fullÚmass_matrix_fullÚ
isinstanceÚmethodr   )	r   r   ZPinZC_ixxÚgr#   r$   r+   Zsoln© r-   ún/var/www/html/bwave/python/venv/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_jointsmethod.pyÚtest_jointsmethod   s(      r/   c               
   C   sÂ   t dƒ\} }}tdƒ}td|d�}tdƒ}tdƒ\}}td||||||  |j |jd�}tƒ � t	||ƒ}	W 5 Q R X |	j
 |j|| |j f¡ |	 ¡  |	 ¡ }
|
d	 | t|ƒ |  ks¾t‚d S )
Núl m gr   Úb©ÚmassÚb_frameúq ur   )ÚcoordinatesÚspeedsZchild_interframeÚchild_pointÚ
joint_axisr   )r   r
   r   r   r   r   ÚxÚzr   r	   r!   Úappendr"   r'   Úrhsr   r   )ÚlÚmr,   r   r1   r4   r#   r$   r   r+   r=   r-   r-   r.   Ú&test_rigid_body_particle_compatibility*   s    
 ÿr@   c               	      s  t ƒ � tdƒ‰ tdƒ} tdƒ}W 5 Q R X tdƒ\}}tdˆ | |ƒ‰td| ||ƒ‰t ƒ � tt‡ ‡‡fdd„ƒ W 5 Q R X tdˆ | |d	�‰td| ||d	�‰t ƒ � tt‡ ‡‡fd
d„ƒ W 5 Q R X tdˆ | ||ƒ‰td| |||ƒ‰t ƒ � tt‡ ‡‡fdd„ƒ W 5 Q R X d S )Nr   r   ÚTr5   r   ÚP2c                      s   t ˆ ˆˆƒS ©N©r	   r-   ©r   r   rB   r-   r.   Ú<lambda>C   ó    z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>)r7   c                      s   t ˆ ˆˆƒS rC   rD   r-   rE   r-   r.   rF   H   rG   c                      s   t ˆ ˆˆƒS rC   rD   r-   rE   r-   r.   rF   M   rG   )r   r   r   r   r   r   Ú
ValueError)r   rA   r#   r$   r-   rE   r.   Ú-test_jointmethod_duplicate_coordinates_speeds:   s"      rI   c               
   C   sB  t dƒ\} }t dƒ\}}tdƒ\}}}tƒ �& tdƒ}td|d�}td|d�}	W 5 Q R X td|||| | |j |jd	�}
td
||	||| |	j |jd	�}| || |j ¡ |	 || |j ¡ tƒ � t||
|ƒ}W 5 Q R X | 	¡  t
|jƒtddddgddddgddd|d  | t|ƒ d|d  |  |d | t|ƒ |d |  gdd|d | t|ƒ |d |  |d | ggƒk�sŠt‚t|jƒtt|g|g| | | t| | ƒt| ƒ  || | t| ƒ  |d | d| |  | t|ƒ  g| | | t| | ƒ |d | |d  t|ƒ  ggƒƒk�s>t‚d S )Nzq1 q2zu1 u2zm l gr   r   r2   ÚRÚJ1©r7   r6   r8   r9   ÚJ2r   r   é   é   )r   r   r   r   r   r:   r;   r   r	   r'   r   r)   r   r   r   r   r(   r   )Úq1Úq2Úu1Úu2r?   r>   r,   r   ZPartPZPartRrK   rM   r+   r-   r-   r.   Ú$test_complete_simple_double_pendulumO   s@    
 ÿ
 ÿ
J.ý4ÿ"ÿ6þrT   c               	   C   s–  t dƒ\} }}}tdƒ\}}}}}tƒ �& tdƒ}	td|d�}
td|d�}W 5 Q R X td|	|
| |d�}td	|
|||d�}|	j||  |	j |
d
� |	j|| |	j |
d
� |
j|| |	j |d
� |
j|| |	j |d
� tƒ � t|	||ƒ}W 5 Q R X | ¡  |j	}|j
}| |¡}t|d ƒt| |  ||  ||  ||  | ƒk�sPt‚t|d ƒt||  ||  d| |  d| |  | ƒk�s’t‚d S )Nzq1 q2 u1 u2zm c1 c2 k1 k2ÚWÚB1r2   ÚB2rK   )r6   r7   rM   )Zreaction_bodyr   r   rN   )r   r   r   r   r   r   r:   r	   r'   Úmass_matrixÚforcingÚLUsolver   r   )rP   rQ   rR   rS   r?   Úc1Úc2Zk1Zk2rU   rV   rW   rK   rM   r+   ÚMMrY   r=   r-   r-   r.   Útest_two_dof_jointsk   s4    
<(ÿÿÿr^   c            	   	   C   sÊ   t dƒ\} }}tƒ � tdƒ}td|d�}W 5 Q R X tdƒ}td||| t¡||  |j |jd�}| | |  t	|ƒ |_
tƒ � t||ƒ}W 5 Q R X | t¡ | ¡ }|d | t|ƒ |  ksÆt‚d S )	Nr0   r   r1   r2   r#   r   rL   r   )r   r   r   r   r   r&   Útr:   r;   r   Zpotential_energyr	   r'   r   r=   r   r   )	r>   r?   r,   r   r1   r#   r   r+   r=   r-   r-   r.   Útest_simple_pedulum‚   s    
 ÿ
r`   c               
   C   s.  t dƒ\	} }}}}}}}}tdƒ\}	}
}}tdƒ}tdƒ}tƒ �B td| |t|||dƒd�}td||t||||ƒd�}td	ƒ}W 5 Q R X td
|||	|| |j |jd�}td|||
||| |j |jd�}| 	| | |j ¡ | 	|| |j ¡ tƒ � t
|||ƒ}W 5 Q R X | ¡  |j}|j}| |¡}d| | | t|
ƒ t|
ƒ d| | | t|
ƒ t|
ƒ  || |  t|	ƒ  || | t|	ƒ  ||t|
ƒd   |t|
ƒd   |d |   |d |   }|d |  ¡ dk�sìt‚|| |d  t|
ƒ t|
ƒ | }|d |  ¡ dk�s*t‚d S )Nz)mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, gztheta phi omega alphaÚAÚBÚrodr   )r3   r   Úcentral_inertiaÚplater   rK   )r6   r7   r8   r9   rM   )r6   r7   Úparent_pointr9   éþÿÿÿrN   r   )r   r   r   r   r   r   r   r;   r   r   r	   r'   rX   rY   rZ   r   r   Úsimplifyr   )ZmAZmBZlAZlBZIAxxZIBxxZIByyZIBzzr,   ÚthetaÚphiÚomegaÚalphara   rb   rc   re   r   rK   rM   r+   r]   rY   r=   Zxdr-   r-   r.   Útest_chaos_pendulum‘   s`    ÿÿ
 ÿ ÿ
4ÿÿÿ
ÿÿ
ÿ
ÿÿ$rm   c            &         sl  t dƒ\} }}}}}tdƒ\}}}}	‰tdƒ‰ ‡ ‡fdd„||||	fD ƒ}
tƒ �` tdˆ ˆ| |
d d�}td	ˆ| |
d
 d�}tdˆ| |
d d�}tdˆ|	 |
d d�}W 5 Q R X td||| ||j|d |j | d |j d�}td|||||j|d |j | d |j d�}td|||||j|d |j |	 d |j d�}|j 	|j¡|d |j  |	d |j  }t
| |j¡| |j¡gƒ}tƒ � t||||ƒ}W 5 Q R X t j}t|j|  |¡| |¡| |¡gƒ}| |¡ |¡}t|j| g|g||g||g|j|||j|jd�
}| ¡ \}}|td
ƒk�s4t‚t
||||	ˆgƒ}t
| ||gƒ}t
|||gƒ}t||f|jƒ}t|||f|jƒ}t|||f|ƒ} dddddg}!dddg}"dddg}#t
d d!d"gd#d$d%gd&d'd(ggƒ}$t
d)gd*gd+ggƒ}%d,‰t‡fd-d.„| |"|#|!ƒD ƒƒ�st‚t‡fd/d.„t
||"|!ƒƒ|$ D ƒƒ�s<t‚t‡fd0d.„t
||"|#|!ƒƒ|% D ƒƒ�sht‚d S )1Nz
q1:4, u1:4z	l1:5, rhoÚNc              	      s&   g | ]}t ˆ d d ˆ|d  d ƒ‘qS )r   rO   é   )r   )Ú.0r>   )rn   Úrhor-   r.   Ú
<listcomp>º   s     zAtest_four_bar_linkage_with_manual_constraints.<locals>.<listcomp>ZLink1r   )r   r3   rd   ZLink2r   )r3   rd   ZLink3rN   ZLink4rO   rK   )r6   r7   r9   rf   r8   rM   ZJ3)	Zq_indZu_indZq_dependentZu_dependentZkd_eqsZconfiguration_constraintsZvelocity_constraintsZ	forcelistr    g¤p=
×£À?g¸…ëQ¸Î?gáz®GáÊ?gÃõ(\�ÂÕ?iå  gÍÌÌÌÌÌ @gÐtM)Lå?gFªÂ!8@gš™™™™™É?g6è8[þÆ¿gàµ„’‡ÁÀ?gD†ÇówCA@gƒ_'@gÆ�Y>ÂÀg    Ð½gTBìÿ�„Ê?gvûÖ1Ò?g_r=
×£À¿g0Ò S‚¿g©;àÞÇ?gàLßýõŸ¿gdžô¶`¢E¿gopÊñž¶]?g»½×Ùß|Û=c                 3   s   | ]}t |ƒˆ k V  qd S rC   ©Úabs©rp   r:   ©Úepsr-   r.   Ú	<genexpr>õ   s     z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>c                 3   s   | ]}t |ƒˆ k V  qd S rC   rs   ru   rv   r-   r.   rx   ö   s     c                 3   s   | ]}t |ƒˆ k V  qd S rC   rs   ru   rv   r-   r.   rx   ø   s     )r   r   r   r   r   r   r;   r:   r"   Zpos_fromr   Údotr   r	   Ú_tr   r%   r&   Úsubsr   r   r!   r    Zkanes_equationsr   r   r   rX   rY   Úall)&rP   rQ   Zq3rR   rS   Úu3Úl1Úl2Úl3Úl4ZinertiasÚlink1Úlink2Zlink3Zlink4Zjoint1Zjoint2Zjoint3ÚloopÚfhr+   r_   ZqdotsZfhdZkaneÚfrZfrsÚpr#   r$   Zeval_mZeval_fZeval_fhdZp_valsZq_valsZu_valsZ
mass_checkZforcing_checkr-   )rn   rw   rq   r.   Ú-test_four_bar_linkage_with_manual_constraintsµ   s¼    ÿ       þ      þ      þÿÿ$   ü

ÿÿÿüþ$ÿÿrˆ   N)+Zsympy.core.functionr   Zsympy.core.symbolr   Z(sympy.functions.elementary.trigonometricr   r   Zsympy.matrices.denser   Zsympy.simplify.trigsimpr   Zsympy.physics.mechanicsr   r	   r
   r   r   r   r   r   r   Zsympy.physics.vectorr   r   Zsympy.testing.pytestr   r   Zsympyr   Zsympy.utilities.lambdifyr   Zsympy.solvers.solversr   rz   r_   r/   r@   rI   rT   r^   r`   rm   rˆ   r-   r-   r-   r.   Ú<module>   s&   ,$