U
    9%e"                     @   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 d dlmZmZ d dl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S )    )expand)symbols)cossin)Matrix)trigsimp)PinJointJointsMethodBodyKanesMethodPrismaticJointLagrangesMethodinertia)dynamicsymbolsReferenceFrame)raises)zeros)lambdify)solvec            	      C   sJ  t d} t d}td| |}td\}}td\}}| || j  t| |}|j| jks^t|j	|| gkspt|j
| j|| jj fgkst|jt|gkst|jt|gkst|jt||  gkst| }|t| |  ggkst|jt|gdggkst|jtddgd|ggks4tt|jtsFtd S )NPCP1zC_ixx gz
q_P1, u_P1r      )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/Darija-Ai-API/env/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_jointsmethod.pytest_jointsmethod   s$    
 r,   c                     s   t d t d} t d}td\}}td | |td| ||tt fdd td | |d	td| ||d	tt fd
d td | ||td| |||tt fdd d S )Nr   r   Tzq ur   P2c                      s   t  S Nr	   r*   r   r   r.   r*   r+   <lambda>-       z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>)speedsc                      s   t  S r/   r0   r*   r1   r*   r+   r2   1   r3   c                      s   t  S r/   r0   r*   r1   r*   r+   r2   5   r3   )r
   r   r   r   r   
ValueError)r   r-   r    r!   r*   r1   r+   -test_jointmethod_duplicate_coordinates_speeds&   s    r6   c               
   C   s  t d\} }t d\}}td\}}}td}td|d}td|d}	td|||| | |j |jd	}
td
||	||| |	j |jd	}||| |j  |	|| |j  t||
|}|  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sft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   massRJ1r4   coordinateschild_point
joint_axisJ2r   r         )r   r   r
   r   xzr   r	   r$   r   r&   r   r   r   r   r%   r   )q1q2u1u2mlr)   r   ZPartPZPartRr:   r?   r(   r*   r*   r+   $test_complete_simple_double_pendulum7   s<    
 
 
J.4"6rJ   c                  C   sr  t d\} }}}td\}}}}}td}	td|d}
td|d}td|	|
| |d}td	|
|||d}|	j||  |	j |
d
 |	j|| |	j |
d
 |
j|| |	j |d
 |
j|| |	j |d
 t|	||}|  |j}|j	}|
|}t|d t| |  ||  ||  ||  | ks,tt|d t||  ||  d| |  d| |  | ksntd S )Nzq1 q2 u1 u2zm c1 c2 k1 k2WB1r7   B2r:   )r<   r4   r?   )Zreaction_bodyr   r   r@   )r   r   r
   r   r   rB   r	   r$   mass_matrixforcingLUsolver   r   )rD   rE   rF   rG   rH   c1c2Zk1Zk2rK   rL   rM   r:   r?   r(   MMrO   rhsr*   r*   r+   test_two_dof_jointsQ   s0    
<(rU   c            	   	   C   s   t d\} }}td}td|d}td}td|||t||  |j |jd}| | |  t| |_	t
||}|t | }|d | t| |  kstd S )	Nzl m gr   br7   r    r   r;   r   )r   r
   r   r   r#   trB   rC   r   Zpotential_energyr	   r$   r   rT   r   r   )	rI   rH   r)   r   rV   r    r   r(   rT   r*   r*   r+   test_simple_pedulumf   s    
 

rX   c               	   C   s
  t d\	} }}}}}}}}td\}	}
}}td}td}td| |t|||dd}td||t||||d}td	}td
|||	|| |j |jd}td|||
||| |j |jd}|| | |j  ||| |j  t	|||}|
  |j}|j}||}d| | | t|
 t|
 d| | | t|
 t|
  || |  t|	  || | t|	  ||t|
d   |t|
d   |d |   |d |   }|d |  dkst|| |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   )r8   r   central_inertiaplater   r:   )r<   r4   r=   r>   r?   )r<   r4   parent_pointr>   r@   r   )r   r   r   r
   r   r   rC   r   r   r	   r$   rN   rO   rP   r   r   simplifyr   )ZmAZmBZlAZlBZIAxxZIBxxZIByyZIBzzr)   thetaphiomegaalpharY   rZ   r[   r]   r   r:   r?   r(   rS   rO   rT   Zxdr*   r*   r+   test_chaos_pendulums   sT    
  
4


$re   c            &         sH  t d\} }}}}}td\}}}}	td  fdd||||	fD }
td | |
d d}td	| |
d
 d}td| |
d d}td|	 |
d d}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 j}t|j| |||||g}|||}t|j| g|g||g||g|j|||j|jd
}| \}}|td
kst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 sttfd/d.t	||"|!|$ D sttfd0d.t	||"|#|!|% D sDtd S )1Nz
q1:4, u1:4z	l1:5, rhoNc              	      s&   g | ]}t  d d |d  d qS )r   rA      )r   ).0rI   )rf   rhor*   r+   
<listcomp>   s     zAtest_four_bar_linkage_with_manual_constraints.<locals>.<listcomp>ZLink1r   )r   r8   r\   ZLink2r   )r8   r\   ZLink3r@   ZLink4rA   r:   )r<   r4   r>   r^   r=   r?   ZJ3)	Zq_indZu_indZq_dependentZu_dependentZkd_eqsZconfiguration_constraintsZvelocity_constraintsZ	forcelistr   gp=
ף?gQ?gzG?g(\?i  g @gtM)L?gF!8@g?g68[ƿgൄ?gDwCA@g_'@gY>g    gTB?gv1?g_r=
ףg0 Sg;?gLgd`Egop]?g|=c                 3   s   | ]}t | k V  qd S r/   absrh   rB   epsr*   r+   	<genexpr>   s     z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>c                 3   s   | ]}t | k V  qd S r/   rk   rm   rn   r*   r+   rp      s     c                 3   s   | ]}t | k V  qd S r/   rk   rm   rn   r*   r+   rp      s     )r   r   r   r
   r   rC   rB   r   Zpos_fromr   dotr   r	   _tr   r"   r#   subsr   r   r   r   Zkanes_equationsr   r   r   rN   rO   all)&rD   rE   Zq3rF   rG   u3l1l2l3l4Zinertiaslink1link2Zlink3Zlink4Zjoint1Zjoint2Zjoint3loopfhr(   rW   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*   )rf   ro   ri   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   Zsympy.physics.vectorr   r   Zsympy.testing.pytestr   Zsympy.core.backendr   Zsympy.utilities.lambdifyr   Zsympy.solvers.solversr   rr   rW   r,   r6   rJ   rU   rX   re   r   r*   r*   r*   r+   <module>   s$   $ 