U
    L?h'                     @   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st	|j
|| gkst	|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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-   a/var/www/html/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st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st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"6rT   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st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st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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   )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    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 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&   ,$