o
    *ήc8$                     @   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   s8  t d} t d}td| |}td\}}td\}}| || j  t| |}|j| jks/J |j|| gks8J |j	| j
|| jj fgksGJ |j|gksOJ |j|gksWJ |j||  gkscJ | }|t| |  ggksvJ |jt|gdggksJ |jtddgd|ggksJ t|jtsJ d S )NPCP1zC_ixx gztheta_P1, omega_P1r      )r
   r   r   r   apply_forceyr	   framebodiesloads
masscenterqukdesdiff	form_eomsr   forcing_fullmass_matrix_full
isinstancemethodr   )	r   r   PinC_ixxgthetaomegar'   soln r.   V/tmp/pip-target-vg8gfxp4/lib/python/sympy/physics/mechanics/tests/test_jointsmethod.pytest_jointsmethod   s$   
r0   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                         t  S Nr	   r.   r   r   r2   r.   r/   <lambda>-       z?test_jointmethod_duplicate_coordinates_speeds.<locals>.<lambda>)speedsc                      r3   r4   r5   r.   r6   r.   r/   r7   1   r8   c                      r3   r4   r5   r.   r6   r.   r/   r7   5   r8   )r
   r   r   r   r   
ValueError)r   r1   r   r    r.   r6   r/   -test_jointmethod_duplicate_coordinates_speeds&   s   r;   c               
   C   s  t d\} }t d\}}td\}}}td}td|d}td|d}	td|||| | |j |j|jd	}
td
||	||| |	j |j|	jd	}||| |j  |	|| |j  t||
|}|  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   massRJ1r9   coordinateschild_joint_posparent_axis
child_axisJ2)r   r   r   r   )r   r   r   r   r         )r   r   r
   r   xzr   r	   r#   r   r%   r   r   r   r$   r   )q1q2u1u2mlr*   r   PartPPartRr?   rE   r'   r.   r.   r/   $test_complete_simple_double_pendulum7   s<   J.4"6rR   c                  C   sn  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sJ t|d t||  ||  d| |  d| |  | ksJ d S )Nzq1 q2 u1 u2zm c1 c2 k1 k2WB1r<   B2r?   )rA   r9   rE   )reaction_bodyr   r   rF   )r   r   r
   r   r   rH   r	   r#   mass_matrixforcingLUsolver   )rJ   rK   rL   rM   rN   c1c2k1k2rS   rT   rU   r?   rE   r'   MMrX   rhsr.   r.   r/   test_two_dof_jointsT   s0   
:(r`   c            	   
   C   s   t d\} }}td}td|d}td}td|||t||  |j |j|jd}| | |  t| |_	t
||}|t | }|d | t| |  ksSJ d S )	Nzl m gr   br<   r   r   r@   r   )r   r
   r   r   r"   trH   rI   r   potential_energyr	   r#   r   r_   r   )	rO   rN   r*   r   ra   r   r   r'   r_   r.   r.   r/   test_simple_pedulumi   s   

"rd   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|jd}td|||
||| |j |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sJ || |d  t|
 t|
 | }|d |  dksJ d S )Nz)mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, gztheta phi omega alphaABrodr   )r=   r   central_inertiaplater   r?   )rA   r9   rB   rC   rD   rE   )rA   r9   parent_joint_posrC   rD   rF   r   )r   r   r   r
   r   r   rI   r   r   r	   r#   rW   rX   rY   r   r   simplify)mAmBlAlBIAxxIBxxIByyIBzzr*   r+   phir,   alphare   rf   rg   ri   r   r?   rE   r'   r^   rX   r_   xdr.   r.   r/   test_chaos_pendulumv   sP   
4


$rx   c            &         sD  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 |j| d |j d	}td|||||j|d |j |j| d |j d	}td|||||j|d |j |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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 suJ tfd!d t	||"|!|$ D sJ 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   rG      )r   ).0rO   )ry   rhor.   r/   
<listcomp>   s   & zAtest_four_bar_linkage_with_manual_constraints.<locals>.<listcomp>Link1r   )r   r=   rh   Link2r   )r=   rh   Link3rF   Link4rG   r?   )rA   r9   rC   rj   rD   rB   rE   J3)	q_indu_indq_dependentu_dependentkd_eqsconfiguration_constraintsvelocity_constraints	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       | ]	}t | k V  qd S r4   absr{   rH   epsr.   r/   	<genexpr>       z@test_four_bar_linkage_with_manual_constraints.<locals>.<genexpr>c                 3   r   r4   r   r   r   r.   r/   r      r   c                 3   r   r4   r   r   r   r.   r/   r      r   )r   r   r   r
   r   rI   rH   r   pos_fromr   dotr   r	   _tr   r!   r"   subsr   r   r   r   kanes_equationsr   r   rW   rX   all)&rJ   rK   q3rL   rM   u3l1l2l3l4inertiaslink1link2link3link4joint1joint2joint3loopfhr'   rb   qdotsfhdkanefrfrspr   r    eval_meval_feval_fhdp_valsq_valsu_vals
mass_checkforcing_checkr.   )ry   r   r|   r/   -test_four_bar_linkage_with_manual_constraints   s~   $$r   N)'sympy.core.functionr   sympy.core.symbolr   (sympy.functions.elementary.trigonometricr   r   sympy.matrices.denser   sympy.simplify.trigsimpr   sympy.physics.mechanicsr   r	   r
   r   r   r   r   sympy.physics.vectorr   r   sympy.testing.pytestr   sympy.core.backendr   sympy.utilities.lambdifyr   sympy.solvers.solversr   r   rb   r0   r;   rR   r`   rd   rx   r   r.   r.   r.   r/   <module>   s&    $ 