o
    *Î®cP  ã                   @   sö   d dl mZ d dlmZ 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 d d	lmZmZmZmZ d d
lmZ d dlmZ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d„ Z'dS ) é    )Ú
expand_mul)Úpi)ÚS)Úsqrt)ÚcosÚsin)ÚMatrix)Ú_simplify_matrix)Úsymbols)ÚdynamicsymbolsÚBodyÚPinJointÚPrismaticJoint©ÚJoint)ÚVectorÚReferenceFrame)Úraisesc                  C   s4   t dƒ} t dƒ}td| d}td|d}| |||fS )NÚNÚAÚP©ÚframeÚC)r   r   )r   r   r   r   © r   úO/tmp/pip-target-vg8gfxp4/lib/python/sympy/physics/mechanics/tests/test_joint.pyÚ_generate_body   s
   r   c                      s(   t dƒ‰t dƒ‰ tt‡ ‡fdd„ƒ d S )NÚparentÚchildc                      s   t dˆˆ ƒS )NÚJr   r   ©r   r   r   r   Ú<lambda>   s    ztest_Joint.<locals>.<lambda>)r   r   Ú	TypeErrorr   r   r    r   Ú
test_Joint   s   r#   c            
      C   sò  t dƒ} t dƒ}tdƒ\}}tdƒ\}}td| |ƒ}|jdks!J ‚|j| ks(J ‚|j|ks/J ‚|j|gks7J ‚|j|gks?J ‚|j	|| 
t¡ gksLJ ‚|j| jjksUJ ‚|j |j¡tdƒksbJ ‚|j | j¡tdƒksoJ ‚|j |j¡tdƒks|J ‚|j | j¡tdƒks‰J ‚| ¡ dks‘J ‚t dƒ}t d	ƒ}td
||||jj ||jj |jjd}	|	j|jjks¶J ‚|	j |j¡||jj ksÅJ ‚|	j |j¡||jj ksÔJ ‚|	j |	j¡tdƒksáJ ‚|j |j¡| |jj ||jj  ks÷J ‚d S )Nr   r   úl mútheta_J, omega_Jr   r   z PinJoint: J  parent: P  child: CÚP1ÚC1ÚJ1©Úparent_joint_posÚchild_joint_posÚparent_axis)r   r
   r   r   Únamer   r   ÚcoordinatesÚspeedsÚkdesÚdiffÚtr,   r   ÚxÚchild_pointÚpos_fromÚ
masscenterr   Úparent_pointÚ_child_pointÚ__str__ÚyÚzÚ_parent_axisÚ_parent_point)
r   r   ÚlÚmÚthetaÚomegaÚPjr&   r'   r(   r   r   r   Útest_pinjoint   s:   ÿÿrC   c               
   C   s€  t dƒ\} }t dƒ\}}tdƒ\}}tdƒ}tdƒ}tdƒ}td|d}	td	||d
}
td||d
}td|	|
|| | |j |	jj|
jjd}td|
|||| |j |
jj|jjd}| |¡t	t
| ƒt| ƒ dgt| ƒt
| ƒdgg d¢gƒksyJ ‚| |¡t	t
|ƒt|ƒ dgt|ƒt
|ƒdgg d¢gƒks˜J ‚t| |¡ƒt	t
| | ƒt| | ƒ dgt| | ƒt
| | ƒdgg d¢gƒksÁJ ‚| |¡||j ksÍJ ‚| |¡||j ksÙJ ‚| |¡||j ||j  ksêJ ‚|j||  t¡ gks÷J ‚|j|| t¡ gksJ ‚|
j |¡|| |j ksJ ‚|j |¡|| |j ks%J ‚|j |¡|| |j |||  |j  ks>J ‚d S )Nzq1 q2zu1 u2zm lr   r   ÚBr   r   r   ©r   ÚmassÚRr(   )r/   r.   r+   r,   Ú
child_axisÚJ2r   ©r   r   é   )r   r
   r   r   r   r3   r   r;   Údcmr   r   r   r	   Ú
ang_vel_inr0   r1   r2   r6   Úvelr:   )Úq1Úq2Úu1Úu2r?   r>   r   r   rD   r   ÚPartPÚPartRr(   rI   r   r   r   Útest_pin_joint_double_pendulum@   sF   þþÿÿ(þ"  6rU   c               
   C   sò  t dƒ\} }}}}tdƒ\}}}}tdƒ}	tdƒ}
tdƒ}||d  d }|d |d  }td|
| d	}td
||d}td|	d}td||||||
j |	j|
jd}td||||||
j |
j|jd}|
 |	¡tt	|ƒdt
|ƒ gg d¢t
|ƒdt	|ƒggƒks‚J ‚|
 |¡tt	|ƒt
|ƒ dgt
|ƒt	|ƒdgg d¢gƒks¡J ‚| |	¡tt	|ƒt	|ƒ t
|ƒt
|ƒ t	|ƒ gt
|ƒ t	|ƒ t	|ƒt
|ƒt
|ƒ gt
|ƒdt	|ƒggƒksÚJ ‚|
 |	¡||	j ksæJ ‚|
 |¡| |
j ksóJ ‚|	 |¡| |	j ||
j  ksJ ‚|j|| t¡ gksJ ‚|j|| t¡ gks"J ‚|j |j¡||
j ks1J ‚|j |j¡| |
j ksAJ ‚|j |	¡|d |d  | |
j ksWJ ‚|j |	¡|d |d  | |d |d  |  |
j kswJ ‚d S )NzmA, mB, lA, lB, hztheta phi omega alphar   r   rD   é   é   ÚrodrE   Úplate)rF   r   r   r   r(   )r.   r/   r+   r,   rH   rI   )r.   r/   r*   r,   rH   r   ©r   rK   r   rJ   )r
   r   r   r   r   r;   r:   rL   r   r   r   rM   r0   r1   r2   r6   r5   rN   r3   )ÚmAÚmBÚlAÚlBÚhr@   ÚphirA   Úalphar   r   rD   ÚlCrX   rY   r   r(   rI   r   r   r   Útest_pin_joint_chaos_pendulumj   sT   ÿÿþþ
&&ý& ,ÿÿrc   c            	      C   s6  t dƒ\} }tƒ \}}}}td|||j d |j  |j¡dks#J ‚|j |¡ |jks/J ‚| |¡tg d¢dt| ƒ t	| ƒ gdt	| ƒ t| ƒggƒksPJ ‚| 
|¡||j ks\J ‚| 
|¡ ¡ t|d ƒkskJ ‚|j |j¡dksvJ ‚|j |j¡ |¡ ¡ dks†J ‚|j |¡dksJ ‚tƒ \}}}}td|||j|jd |j |j¡dks­J ‚|j |¡|jks¸J ‚| |¡tdt| ƒ t	| ƒ gg d¢dt	| ƒ t| ƒggƒksÙJ ‚| 
|¡||j ksåJ ‚| 
|¡ |¡||j ksôJ ‚| 
|¡ ¡ t|d ƒksJ ‚| 
|¡ |j¡dksJ ‚|j |¡||j ks J ‚|j |j¡|j ks.J ‚|j |j¡ |¡ ¡ t| ƒ|j t	| ƒ|j  ksKJ ‚|j |¡ |j¡td ks\J ‚tƒ \}}}}td|||j|jd	 |j |j¡dkszJ ‚|j |¡|jks†J ‚| |¡tg d
¢t| ƒ dt	| ƒgt	| ƒdt| ƒggƒks¦J ‚| 
|¡||j ks³J ‚| 
|¡ |¡||j ksÃJ ‚| 
|¡ ¡ t|d ƒksÓJ ‚| 
|¡ |j¡}| |di¡dksèJ ‚|j |¡dksóJ ‚|j |j¡|jks J ‚tƒ \}}}}td|||j|j|j|j d t|j |j|j ¡ƒdks(J ‚|j|j  |¡ ¡ tdƒ|j ks=J ‚t| |¡ƒttdƒd tdƒ t| ƒ d tdƒ t	| ƒ d gtdƒd tdƒt| ƒ d tdƒt	| ƒ d gdt	| ƒ t| ƒggƒks†J ‚| 
|¡||j ks“J ‚| 
|¡ |¡ ¡ ||j ||j  tdƒ ks®J ‚| 
|¡ ¡ t|d ƒks¾J ‚| 
|¡ |j|j ¡}| |di¡dksÖJ ‚|j |¡ ¡ ||j tdƒ ksêJ ‚|j |j¡|j|j ksúJ ‚|j |j¡ |¡ ¡ dtdƒd  |j tdƒt| ƒ d |j  tdƒt	| ƒ d |j  ks.J ‚|j |¡ |¡ ¡ tdƒ | t	| ƒ d |j tdƒ| t| ƒ d |j  ks[J ‚|j |¡ |j¡td kslJ ‚tƒ \}}}}td|||j|j|j|j |j d t|j |j|j |j ¡ƒdksšJ ‚|j|j |j  |¡ ¡ tdƒ|j ks²J ‚t| |¡ƒttdƒd tdƒ t	| td  ƒ d tdƒt| td  ƒ d gtdƒd tdƒt| td  ƒ d tdƒt	| td  ƒ d gtdƒ d tdƒt| dt d  ƒ d tdƒt	| dt d  ƒ d ggƒks&J ‚| 
|¡||j ks3J ‚| 
|¡ |¡ ¡ ||j ||j  ||j  tdƒ ksSJ ‚| 
|¡ ¡ t|d ƒkscJ ‚| 
|¡ |j|j |j ¡}| |di¡dks~J ‚|j |¡ ¡ ||j ||j  tdƒ ks—J ‚|j |j¡|j|j ks§J ‚|j |j¡ |¡ ¡ dtdƒd  |j tdƒt	| td  ƒ d |j  tdƒt| td  ƒ d |j  ksãJ ‚|j |¡ |¡ ¡ tdƒ| t| td  ƒ d |j tdƒ| t	| td  ƒ d |j  ksJ ‚|j |¡ |j¡td ks(J ‚tƒ \}}}}tdƒ\}}td||||j ||j |j|j |j |j|j |j d |j|j |j  |j|j |j ¡}t|ƒdkspJ ‚|j|j |j  |¡ ¡ dt| ƒ d tdƒd  |j tdƒd dt	| td  ƒ d  |j  dt| td  ƒ d tdƒd  |j  ks¼J ‚t| |¡ƒttdƒd dt| ƒ d  dt	| td  ƒ d tdƒd  dt| td  ƒ d tdƒd  gdt| td  ƒ d tdƒd  dt| ƒ d tdƒd  dt	| td  ƒ d tdƒd  gdt	| td  ƒ d tdƒd  dt| td  ƒ d tdƒd  dt| ƒ d tdƒd  ggƒksZJ ‚| 
|¡||j ||j  ||j  tdƒ ksuJ ‚| 
|¡ |¡ ¡ ||j ||j  ||j  tdƒ ks•J ‚| 
|¡ ¡ t|d ƒks¥J ‚| 
|¡ |j|j |j ¡}| |di¡dksÀJ ‚|j |¡ ¡ tdƒ| | d |j tdƒ| | d |j  ksåJ ‚|j |j¡||j ||j  ksùJ ‚|j |j¡ |¡ ¡ ||dt| ƒ d  d  |j |dt	| td  ƒ d  d |j  |dt| td  ƒ d  d |j  ks?J ‚|j |¡ |¡ ¡ d| | t	| ƒ d |j d| | t| td  ƒ d |j  d| | t	| td  ƒ d |j  ks‚J ‚|j |¡ |j|j |j ¡ ¡ dks™J ‚d S )Nr%   r   ©rH   r   ©éÿÿÿÿr   r   rV   ©rH   r+   ©rK   r   r   ©r,   r*   rZ   rK   ©r*   r+   rH   é   é   rW   é   é   úm n©r*   r+   rH   r,   éüÿÿÿéþÿÿÿ)r   r   r   r3   Úangle_betweenÚexpressrL   r   r   r   rM   Ú	magnituder   r6   r5   ÚsimplifyrN   r:   Úcrossr;   r   Úxreplacer   r	   r
   r   Údot)	r@   rA   r   r   r   r   Úangler?   Únr   r   r   Útest_pinjoint_arbitrary_axis™   sP  þ þ 
ÿ"þ  
ÿ"*40ý
ÿ ( .ÿ
ÿ<
ÿ"ÿ(0&ÿ$ÿ*ÿû$ÿÿ 2 6 ÿ
ÿ$$ÿ
ÿ" ÿ&J(ÿ
ÿ<"ÿ<"ÿDÿû6$ÿÿ 2
ÿ(4ÿÿÿ$ÿ
ÿB$ÿ
ÿ2r|   c                  C   s
  t ƒ \} } }}td|||jj d}| ¡ |jjksJ ‚t ƒ \} } }}td|||jj|jj d}| ¡ |jjks;J ‚t ƒ \} } }}td|||jj|jj d}| ¡ |jjksZJ ‚t ƒ \} } }}td|||jj|jj |jj |jj d}| ¡ |jjksJ ‚t ƒ \} } }}td|||jj|jj |jj |jj d}| ¡ |jjks¨J ‚t ƒ \} } }}td|||jj|jj |jj |jj d}| ¡ |jjksÏJ ‚t ƒ \} } }}td|||jj|jj |jj |jj |jj |jj d}| ¡ |jj|jj ksJ ‚d S )Nr   rd   )r,   rH   )r   r   r   r3   Ú_generate_vectorr;   r:   )Ú_r   r   r   r   r   r   Útest_pinjoint_pi'  s.   ,,,ÿ"r   c            
      C   sÎ  t ƒ \} } }}tdƒ\}}td||ƒ}|jdksJ ‚|j|ks!J ‚|j|ks(J ‚|j|gks0J ‚|j|gks8J ‚|j|| 	t
¡ gksEJ ‚|j|jjksNJ ‚|j|jjksWJ ‚|j |j¡tdƒksdJ ‚|j |j¡tdƒksqJ ‚|j |j¡| |jj ksJ ‚|j |j¡| |jj ks‘J ‚|j |j¡||jj ks J ‚| |¡dks©J ‚| |¡dks²J ‚| ¡ dksºJ ‚t ƒ \}}}}tdƒ\}}	td||||jj |	|jj |jjd}|j|jjksäJ ‚|j |j¡|	|jj ksóJ ‚|j |j¡||jj ksJ ‚|j |j¡| |jj ksJ ‚|j |j¡| |j ||j  |	|j  ks.J ‚|j |j¡||jj ks>J ‚| |¡dksHJ ‚| |¡dksRJ ‚t ƒ \} } }}td||||jj |	|jj |jjd}|j|jjkswJ ‚|j |j¡|	|jj ks‡J ‚|j |j¡||jj ks—J ‚|j |j¡| |jj ks¨J ‚|j |j¡| | |jj |	|jj  ksÁJ ‚|j |j¡||jj ksÑJ ‚| |¡dksÛJ ‚| |¡dksåJ ‚d S )Núx_S, v_Sr   r   z&PrismaticJoint: S  parent: P  child: Cr$   r)   )r   r   r   r-   r   r   r.   r/   r0   r1   r2   r,   r   r3   rH   r4   r5   r6   r   r7   rN   rM   r9   r
   r:   r;   )
r~   r   r   r3   Úvr   r   r   r>   r?   r   r   r   Útest_slidingjointF  sX     ÿ "4 ÿ  "2 r‚   c               	   C   sJ
  t dƒ\} }tƒ \}}}}td|||j d |j  |j¡dks#J ‚|j |¡ |jks/J ‚| |¡tg d¢g d¢g d¢gƒksCJ ‚|j 	|j¡| |j ksQJ ‚|j 	|j¡ |¡ 
¡ |  |j kseJ ‚|j |¡||j ksrJ ‚|j |¡ |¡| |j ksƒJ ‚| |¡dksŒJ ‚| |¡dks•J ‚tƒ \}}}}td|||j|jd |j |j¡dks²J ‚|j |¡|jks½J ‚| |¡tg d¢g d	¢g d¢gƒksÑJ ‚|j |¡||j ksÞJ ‚|j |¡ |¡||j ksîJ ‚|j 	|j¡| |j |j ksÿJ ‚|j 	|j¡ |¡ 
¡ | |j |j ksJ ‚| |¡dks J ‚| |¡dks*J ‚tƒ \}}}}td|||j|jd
 |j |j¡dksHJ ‚|j |¡|jksTJ ‚| |¡tg d¢g d¢g d¢gƒksiJ ‚|j |¡||j kswJ ‚|j |¡ |¡||j ksˆJ ‚|j 	|j¡|j| |j  ksšJ ‚| |¡dks¤J ‚| |¡dks®J ‚tƒ \}}}}td|||j|j|j|j d |j |j|j ¡dksÔJ ‚|j|j  |¡tdƒ|j ksçJ ‚| |¡ttdƒd tdƒ d dgtdƒd tdƒd dgg d¢gƒksJ ‚|j 	|j¡| d |j |j ks#J ‚|j 	|j¡ |¡| tdƒd  d |j tdƒd |j  ksFJ ‚|j |¡ |¡||j|j  tdƒ ks^J ‚|j |¡||j kslJ ‚| |¡dksvJ ‚| |¡dks€J ‚tƒ \}}}}td|||j|j|j|j |j d |j |j|j |j ¡dks¬J ‚|j|j |j  |¡tdƒ|j ksÂJ ‚t| |¡ƒttdƒd tdƒ d tdƒd gtdƒd tdƒd tdƒd  tdƒd tdƒd  gtdƒ d tdƒd tdƒd  tdƒd tdƒd  ggƒksJ ‚|j 	|j¡| d |j |j ks.J ‚|j 	|j¡ |¡| tdƒd  d |j tdƒd |j  tdƒd |j  ksZJ ‚|j |¡||j kshJ ‚|j |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j  ks•J ‚| |¡dksŸJ ‚| |¡dks©J ‚tƒ \}}}}tdƒ\}}td||||j ||j |j|j |j |j|j |j d |j|j |j  |j|j |j ¡dksíJ ‚|j|j |j  |¡|j|j |j ksJ ‚t| |¡ƒttdƒ d tdƒ d tdƒd gtdƒd tdƒd tdƒd gtdƒ d tdƒd tdƒd ggƒksFJ ‚|j 	|j¡|tdƒ|  d  |j tdƒ|  d |j  tdƒ|  d |j  ||j  ksxJ ‚|j 	|j¡ |¡||d  tdƒ|  d  |j d| d tdƒ|  d  |j  d| d tdƒ|  d  |j  ks¸J ‚|j |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j  ksâJ ‚|j |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j  ksJ ‚| |¡dksJ ‚| |¡dks#J ‚d S )Nr€   r   rd   r   re   )r   rf   r   rJ   rg   rh   ri   rZ   rj   rV   rK   rk   rl   ro   rp   rr   )r   r   r   r3   rs   rt   rL   r   r6   r5   rv   rN   rM   r:   r   r;   r	   r   r
   )r3   r   r   r   r   r   r?   r{   r   r   r   Ú test_slidingjoint_arbitrary_axisv  s¢   (("( ".*"$ &P(F0&$,08:þ(<
ÿZ ÿ002 "þN
ÿd
ÿTZrƒ   N)(Úsympy.core.functionr   Úsympy.core.numbersr   Úsympy.core.singletonr   Ú(sympy.functions.elementary.miscellaneousr   Ú(sympy.functions.elementary.trigonometricr   r   Úsympy.matrices.denser   Úsympy.core.backendr	   Úsympy.core.symbolr
   Úsympy.physics.mechanicsr   r   r   r   Úsympy.physics.mechanics.jointr   Úsympy.physics.vectorr   r   Úsympy.testing.pytestr   Ú_tr2   r   r#   rC   rU   rc   r|   r   r‚   rƒ   r   r   r   r   Ú<module>   s.    !*/ 0