a
    kh'                     @   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   s  t    td} td}W d    n1 s,0    Y  td| |}td\}}td\}}| || j  t   t| |}W d    n1 s0    Y  |j| jksJ |j	|| gksJ |j
| j|| jj fgksJ |jt|gksJ |jt|gksJ |jt||  gks"J | }|t| |  ggksJJ |jt|gdggksfJ |jtddgd|ggksJ t|jtsJ d S )NPCP1zC_ixx gz
q_P1, u_P1r      )r   r   r   r   r   apply_forceyr	   frame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,   ]/var/www/auris/lib/python3.9/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 d    n1 s~0    Y  |	j
|j|| |j f |	  |	 }
|
d	 | t| |  ksJ 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   )lmr+   r   r0   r3   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 d    n1 s40    Y  td\}}td | |td| ||t  & tt fdd W d    n1 s0    Y  td | |d	td| ||d	t  & tt fd
d W d    n1 s0    Y  td | ||td| |||t  & tt fdd W d    n1 sR0    Y  d S )Nr   r   Tr4   r   P2c                      s   t  S Nr	   r,   r   r   rA   r,   r-   <lambda>C       z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>)r6   c                      s   t  S rB   rC   r,   rD   r,   r-   rE   H   rF   c                      s   t  S rB   rC   r,   rD   r,   r-   rE   M   rF   )r   r   r   r   r   r   
ValueError)r   r@   r"   r#   r,   rD   r-   -test_jointmethod_duplicate_coordinates_speeds:   s"    &44rH   c               
   C   sb  t d\} }t d\}}td\}}}t 0 td}td|d}td|d}	W d    n1 sb0    Y  td|||| | |j |jd	}
td
||	||| |	j |jd	}||| |j  |	|| |j  t  t||
|}W d    n1 s0    Y  |	  t
|jtg dg dddd|d  | t| d|d  |  |d | t| |d |  gdd|d | t| |d |  |d | ggksJ t|jtt|g|g| | | t| | t|   || | t|   |d | d| |  | t|  g| | | t| |  |d | |d  t|  ggks^J d S )Nzq1 q2zu1 u2zm l gr   r   r1   RJ1r6   r5   r7   r8   J2)r   r   r   r   )r   r   r   r   r         )r   r   r   r   r   r9   r:   r   r	   r&   r   r(   r   r   r   r'   r   )q1q2u1u2r>   r=   r+   r   ZPartPZPartRrJ   rL   r*   r,   r,   r-   $test_complete_simple_double_pendulumO   s<    **J.4"6rS   c                  C   s  t d\} }}}td\}}}}}t 0 td}	td|d}
td|d}W d    n1 s^0    Y  td|	|
| |d}td	|
|||d}|	j||  |	j |
d
 |	j|| |	j |
d
 |
j|| |	j |d
 |
j|| |	j |d
 t  t|	||}W d    n1 s0    Y  |  |j	}|j
}||}t|d t| |  ||  ||  ||  | kszJ t|d t||  ||  d| |  d| |  | ksJ d S )Nzq1 q2 u1 u2zm c1 c2 k1 k2WB1r1   B2rJ   )r5   r6   rL   )Zreaction_bodyr   r   rM   )r   r   r   r   r   r   r9   r	   r&   mass_matrixforcingLUsolver   )rO   rP   rQ   rR   r>   Zc1c2Zk1Zk2rT   rU   rV   rJ   rL   r*   MMrX   r<   r,   r,   r-   test_two_dof_jointsk   s4    *,
<(r\   c            	   	   C   s   t d\} }}t $ td}td|d}W d    n1 s>0    Y  td}td|||t||  |j |jd}| | |  t	| |_
t  t||}W d    n1 s0    Y  |t | }|d | t| |  ksJ d S )	Nr/   r   r0   r1   r"   r   rK   r   )r   r   r   r   r   r%   tr9   r:   r   Zpotential_energyr	   r&   r   r<   r   )	r=   r>   r+   r   r0   r"   r   r*   r<   r,   r,   r-   test_simple_pedulum   s    *(
r^   c               
   C   sX  t d\	} }}}}}}}}td\}	}
}}td}td}t L td| |t|||dd}td||t||||d}td	}W d    n1 s0    Y  td
|||	|| |j |jd}td|||
||| |j |jd}|	| | |j  |	|| |j  t  t
|||}W d    n1 s00    Y  |  |j}|j}||}d| | | t|
 t|
 d| | | t|
 t|
  || |  t|	  || | t|	  ||t|
d   |t|
d   |d |   |d |   }|d |  dksJ || |d  t|
 t|
 | }|d |  dksTJ d S )Nz)mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, gztheta phi omega alphaABrodr   )r2   r   central_inertiaplater   rJ   )r5   r6   r7   r8   rL   )r5   r6   parent_pointr8   rM   r   )r   r   r   r   r   r   r   r:   r   r   r	   r&   rW   rX   rY   r   r   Zsimplify)ZmAZmBZlAZlBZIAxxZIBxxZIByyZIBzzr+   thetaphiomegaalphar_   r`   ra   rc   r   rJ   rL   r*   r[   rX   r<   Zxdr,   r,   r-   test_chaos_pendulum   s\    &,
4


$rj   c            &         s  t d\} }}}}}td\}}}}	td  fdd||||	fD }
t j td | |
d d}td	| |
d
 d}td| |
d d}td|	 |
d d}W d    n1 s0    Y  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 d    n1 s0    Y  t j}t|j| |||||g}|||}t|j| g|g||g||g|j|||j|jd
}| \}}|td
ks^J t
||||	g}t
| ||g}t
|||g}t||f|j}t|||f|j}t|||f|} g d}!g d}"g d}#t
g dg dg dg}$t
dgdgdgg}%dtfdd | |"|#|!D s,J tfd!d t
||"|!|$ D sVJ tfd"d t
||"|#|!|% D sJ d S )#Nz
q1:4, u1:4z	l1:5, rhoNc              	      s&   g | ]}t  d d |d  d qS )r   rN      )r   ).0r=   )rk   rhor,   r-   
<listcomp>   rF   zAtest_four_bar_linkage_with_manual_constraints.<locals>.<listcomp>ZLink1r   )r   r2   rb   ZLink2r   )r2   rb   ZLink3rM   ZLink4rN   rJ   )r5   r6   r8   rd   r7   rL   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 rB   absrm   r9   epsr,   r-   	<genexpr>   rF   z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>c                 3   s   | ]}t | k V  qd S rB   rp   rr   rs   r,   r-   ru      rF   c                 3   s   | ]}t | k V  qd S rB   rp   rr   rs   r,   r-   ru      rF   )r   r   r   r   r   r   r:   r9   r!   Zpos_fromr   dotr   r	   _tr   r$   r%   subsr   r   r    r   Zkanes_equationsr   r   rW   rX   all)&rO   rP   Zq3rQ   rR   u3l1l2l3l4ZinertiasZlink1Zlink2Zlink3Zlink4Zjoint1Zjoint2Zjoint3Z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,   )rk   rt   rn   r-   -test_four_bar_linkage_with_manual_constraints   s    4.$$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   rw   r]   r.   r?   rH   rS   r\   r^   rj   r   r,   r,   r,   r-   <module>   s&   ,$