U
    ZÊ¹jBâ  ã                   @   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mZmZ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mZmZ d d	lmZ d d
l m!Z!m"Z"m#Z# d dl$m%Z%m&Z& ej'Z(dFdd„Z)dd„ Z*dd„ Z+dd„ Z,dd„ Z-dd„ Z.dd„ Z/dd„ Z0dd„ Z1dd „ Z2d!d"„ Z3d#d$„ Z4d%d&„ Z5d'd(„ Z6d)d*„ Z7d+d,„ Z8d-d.„ Z9d/d0„ Z:d1d2„ Z;d3d4„ Z<d5d6„ Z=d7d8„ Z>d9d:„ Z?d;d<„ Z@d=d>„ ZAd?d@„ ZBdAdB„ ZCdCdD„ ZDdES )Gé    )Ú
expand_mul)Úpi)ÚS)Úsqrt)ÚcosÚsin)ÚMatrixÚsimplifyÚeyeÚzeros)Úsymbols)ÚdynamicsymbolsÚ	RigidBodyÚParticleÚJointsMethodÚPinJointÚPrismaticJointÚCylindricalJointÚPlanarJointÚSphericalJointÚ	WeldJointÚBody©ÚJoint)ÚVectorÚReferenceFrameÚPoint)ÚraisesÚwarns_deprecated_sympyFc                 C   s€   t dƒ}t dƒ}td|d�}td|d�}| rtt dƒt dƒ }}| ||jt¡ | ||jt d ¡ ||||||fS ||||fS )	NÚNÚAÚP©ÚframeÚCÚP_intZC_inté   )r   r   Úorient_axisÚxr   Úy)Z
interframer   r    r!   r$   ÚPintÚCint© r,   úg/var/www/html/bwave/python/venv/lib/python3.8/site-packages/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,   ©r0   r/   r,   r-   Ú<lambda>#   ó    ztest_Joint.<locals>.<lambda>)r   r   Ú	TypeErrorr,   r,   r3   r-   Ú
test_Joint    s    r7   c                     s0  t dƒ\} }}}t dƒ\}‰}}}}}	}
t dƒ\‰‰}}‰‰}}tƒ \}}‰‰ tdˆˆ ƒ‰ˆ | d¡t| gƒkstt‚ˆ |gd¡t|gƒks�t‚ˆj|gddd�t|gƒks°t‚ˆ d d¡t|gƒksÊt‚ˆ d gd¡t|gƒksæt‚ˆ ˆd d gd¡tˆˆ|gƒk�st‚ˆ d d¡t|ˆ|gƒk�s,t‚ˆ g d¡t|ˆ|gƒk�sLt‚ˆjg ddd�tˆ||gƒk�spt‚ˆjˆd |gddd�tˆ||gƒk�sšt‚ˆjd ddd�t||gƒk�s¼t‚ˆ d dd	¡t|gƒk�sÚt‚ˆ g dd	¡t|||	gƒk�süt‚ˆjd dd
d�t|gƒk�st‚ˆ g dd	dd
¡t|	gƒk�s>t‚ˆ g dd¡t|ˆ|gƒk�s`t‚tt‡‡‡fdd„ƒ tt‡‡‡fdd„ƒ tt‡‡‡fdd„ƒ tt‡‡fdd„ƒ tt‡‡‡fdd„ƒ tƒ \}}‰‰ tdˆˆ ˆˆ 	t
¡ƒ tƒ \}}‰‰ tt‡ ‡‡fdd„ƒ tt‡ ‡‡‡fdd„ƒ d S )Nzq u q_J u_Jzq0:4_J u0:4_Jz	q0:4 u0:4r2   é   r&   )Úoffseté   ÚuT)Znumber_singleÚqc                      s   ˆ   ˆˆgd¡S )Nr8   ©Ú_fill_coordinate_listr,   ©r2   Úq0Úq1r,   r-   r4   F   r5   z,test_coordinate_generation.<locals>.<lambda>c                      s   ˆ   ˆˆd gdd¡S )Nr&   r;   r=   r,   )r2   Úu0Úu1r,   r-   r4   G   r5   c                      s   ˆ   ˆˆgd¡S )Nr:   r=   r,   r?   r,   r-   r4   H   r5   c                      s   ˆ   ˆtdƒgd¡S )NrA   r&   )r>   r   r,   )r2   r@   r,   r-   r4   J   r5   c                      s   ˆ   ˆˆ ˆgd¡S )Nr&   r=   r,   r?   r,   r-   r4   K   r5   c                      s   t dˆˆ ˆd d gƒS r1   ©r   r,   )r$   r!   Úq1jr,   r-   r4   Q   r5   c                      s   t dˆˆ ˆˆˆgd�S )Nr2   )ÚspeedsrD   r,   )r$   r!   rB   rC   r,   r-   r4   R   r5   )r   r.   r   r>   r   ÚAssertionErrorr   Ú
ValueErrorr6   ÚdiffÚt)r<   r;   ZqjZujZq0jZq2jZq3jZu0jZu1jZu2jZu3jÚq2Úq3Úu2Úu3Ú_r,   )r$   r2   r!   r@   rA   rE   rB   rC   r-   Útest_coordinate_generation&   sF     ÿ  $ÿ"" ""rP   c                  C   s  t dƒ} t dƒ}tdƒ\}}tdƒ\}}td| |ƒ}|jdksBt‚|j| ksPt‚|j|ks^t‚|jt	|gƒksrt‚|j
t	|gƒks†t‚|jt	|| t¡ gƒks¤t‚|j| jjks¶t‚|j |j¡tdƒksÐt‚|j | j¡tdƒksêt‚|j |j¡tdƒk�st‚|j | j¡tdƒk�s"t‚|j| jk�s4t‚|j|jk�sFt‚| ¡ dk�sXt‚t dƒ}t d	ƒ}td
ƒ}	|	 |j|jtd ¡ td||||jj ||jj |jj|	d�}
|
j|jjk�sÄt‚|
j |j¡||jj k�sät‚|
j  |j¡||jj k�st‚|
j  |
j¡tdƒk�s t‚|j |j¡| |jj ||jj  k�sNt‚|
j|	k�s^t‚|
j|jk�spt‚tdƒ\}}t!dƒ\}}} }}	}| j "d|j|j ¡}|j "d|j|j ¡}td| ||||||	||jd�
}|j|jk�sìt‚|j #|¡dk�st‚|j|k�st‚|j|k�s"t‚|j | j¡|j|j k�sBt‚|j |j¡|j|j k�sbt‚|j | j¡|j|j |j |j k�sŽt‚|j #|¡ $|¡|t%|ƒ |t&|ƒ  |j | t%|ƒ |t&|ƒ  |j  k�sät‚|j|	k�sôt‚|j|k�st‚d S )Nr!   r$   úl múq_J, u_Jr2   r   z PinJoint: J  parent: P  child: CÚP1ÚC1r%   r&   ÚJ1©Úparent_pointÚchild_pointÚ
joint_axisÚparent_interframeúq, uTrW   rX   ©rW   rX   rZ   Úchild_interframerY   )'r   r   r   r   ÚnamerG   r/   r0   Úcoordinatesr   rF   ÚkdesrI   rJ   rY   r#   r(   rX   Úpos_fromÚ
masscenterr   rW   Ú_child_pointrZ   r]   Ú__str__r   r'   r)   r   ÚzZ_joint_axisZ_parent_pointr.   Ú	locatenewÚvelÚexpressr   r   )r!   r$   ÚlÚmr<   r;   ZPjrS   rT   r*   rU   r   r    r+   rW   rX   r2   r,   r,   r-   Útest_pin_jointU   sx    
 þ  ÿ
  þ  ,, ÿrk   c                  C   s6  t dƒ\} }tdƒ}tdƒ}td| d�}tdƒ\}}td|||||||j d�}|j|ks^t‚|jj	dksnt‚|j
 |j
¡| |j ksŒt‚| |j¡td	d
d
gd
t|ƒt|ƒgd
t|ƒ t|ƒggƒksÎt‚|j
 |j¡| | |j ksðt‚tdƒ}tdƒ}tdƒ}td| d�}tdƒ\}}td||||||||j |jd�	}|j|jjk�sVt‚| |j¡tt|ƒt|ƒd
gt|ƒ t|ƒd
gd
d
d	ggƒk�sšt‚|j
 |j¡d
k�s²t‚|j
 |j¡|| |j k�sÔt‚tdƒ\}	}
}}}}dd„ |	|f|
|f||ffD ƒ}tdƒtdƒ }}td||||ƒ}|j |j¡tdƒk�sFt‚|j
 |j
¡||jj k�sft‚|j
 |j¡d
k�s~t‚|j
 |j¡||jj k�sžt‚tdƒtdƒ }}tdƒ}td|||	|
|||||j |jd�
}|j |j¡tt|	ƒd
t|	ƒgd
d	d
gt|	ƒ d
t|	ƒggƒk�s"t‚|j
 |j
¡||j |
|j  k�sJt‚|j
 |j¡||j k�sht‚|j
 |j¡ |¡| |j || |j  k�sœt‚tdƒtdƒ }}tdƒ}td|||	|
|g|||g|||j d�	}|j}|j |j¡td	d
d
gd
t|	ƒt|	ƒ gd
t|	ƒt|	ƒggƒk�s*t‚|j
 |j
¡| |j |
|j  ||j  k�s^t‚|j
 |j¡|| |j ||j  ||j  k�s”t‚tdƒtdƒ }}tdƒtdƒ }}td||||||j ||j d�}| |¡tdƒk�sòt‚|j
 |j
¡||j ||j  k�st‚|j
 |j¡d
k�s2t‚d S )Núm lr$   r!   )Úmassr[   r2   ©r]   rX   Z	J_P_framer8   r   )rZ   r]   rX   rY   z	q1:4 u1:4c                 S   s   i | ]\}}|  t¡|“qS r,   )rI   rJ   )Ú.0ZqiZuir,   r,   r-   Ú
<dictcomp>­   s      z/test_particle_compatibility.<locals>.<dictcomp>r:   ÚP_frame)rZ   rW   rY   ÚC_frame)rZ   r]   rW   rX   )r   r   r   r   r   r)   r]   rG   rZ   r^   rb   ra   Údcmr   r   r   rg   re   rY   r(   r   r
   r   Úxreplacer   r   )rj   ri   rr   r!   r$   r<   r;   r2   rq   rA   rK   rL   rC   rM   rN   Z	qdot_to_ur,   r,   r-   Útest_particle_compatibilityŽ   sª    ÿþ" þþ"    ÿ  ÿ(ÿ
 ÿ  ÿÿ
 ÿ

 þ(ru   c               	   C   s
  t dƒ\} }tdƒ}tƒ � tdƒ}td| |d�}W 5 Q R X tdƒ\}}td||||||j d� |j|kspt‚|jj	dks€t‚|j
 |j
¡| |j ksžt‚|j |j¡td	d
d
gd
t|ƒt|ƒgd
t|ƒ t|ƒggƒksât‚|j
 |j¡| | |j k�st‚d S )Nrl   r$   r!   ©rm   r#   r[   r2   ©rX   rq   r8   r   )r   r   r   r   r   r   r)   r#   rG   r^   rb   ra   rs   r   r   r   rg   re   )rj   ri   rr   r!   r$   r<   r;   r,   r,   r-   Útest_body_compatibility×   s    þrx   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d�}td|
|||| |j |
jjd�}| |¡t	t
| ƒt| ƒ dgt| ƒt
| ƒdgdddggƒksèt‚| |¡t	t
|ƒt|ƒ dgt|ƒt
|ƒdgdddggƒk�s*t‚t| |¡ƒt	t
| | ƒt| | ƒ dgt| | ƒt
| | ƒdgdddggƒk�s€t‚| |¡||j k�sšt‚| |¡||j k�s´t‚| |¡||j ||j  k�sØt‚|jt	||  t¡ gƒk�søt‚|jt	|| t¡ gƒk�st‚|
j |¡|| |j k�s8t‚|j |¡|| |j k�sXt‚|j |¡|| |j |||  |j  k�sŠt‚d S )Nzq1 q2zu1 u2rl   r   r    ÚBr$   r"   r!   ©r#   rm   ÚRrU   )rF   r_   rX   rY   ÚJ2r   r8   )r   r   r   r   r   r(   r#   re   rs   r   r   r   rG   r	   Ú
ang_vel_inr`   rI   rJ   rb   rg   r)   )rA   rK   rC   rM   rj   ri   r   r    ry   r$   ZPartPZPartRrU   r|   r,   r,   r-   Útest_pin_joint_double_pendulumè   sJ    
 ÿ
 ÿ ÿ ÿ(þ$    r~   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d�}td||||||
j |
jd�}|
 |	¡tt	|ƒdt
|ƒ gdddgt
|ƒdt	|ƒggƒksþt‚|
 |¡tt	|ƒt
|ƒ dgt
|ƒt	|ƒdgdddggƒk�s@t‚| |	¡tt	|ƒt	|ƒ t
|ƒt
|ƒ t	|ƒ gt
|ƒ t	|ƒ t	|ƒt
|ƒt
|ƒ gt
|ƒdt	|ƒggƒk�s´t‚|
 |	¡||	j k�sÎt‚|
 |¡| |
j k�sêt‚|	 |¡| |	j ||
j  k�st‚|jt|| t¡ gƒk�s0t‚|jt|| t¡ gƒk�sPt‚|j |j¡||
j k�snt‚|j |j¡| |
j k�sŽt‚|j |	¡|d |d  | |
j k�sºt‚|j |	¡|d |d  | |d |d  |  |
j k�sút‚d S )NzmA, mB, lA, lB, hztheta phi omega alphar   r    ry   r&   é   Úrodrz   Úplaterv   r$   r"   rU   )r_   rF   rX   rY   r|   )r_   rF   rW   rY   r   r8   )r   r   r   r   r   re   r)   rs   r   r   r   rG   r}   r`   rI   rJ   rb   ra   rg   r(   )ZmAZmBZlAZlBÚhÚthetaÚphiÚomegaÚalphar   r    ry   ZlCr€   r�   r$   rU   r|   r,   r,   r-   Útest_pin_joint_chaos_pendulum  sX     ÿ ÿþþ
&&ý&   ,ÿÿr‡   c                     sz  t dƒ\} }tƒ \}}‰‰ tdƒtdƒ ‰‰tt‡ ‡‡fdd„ƒ tt‡ ‡‡fdd„ƒ ˆ ||j| ¡ ˆ ||j| ¡ tt‡ ‡‡fdd„ƒ tt‡ ‡‡fdd„ƒ tƒ \}}‰‰ tdƒ‰ˆ |td	 ttd
 fd¡ t	dˆˆ | ||j
ˆ j ˆˆj
d�	 t| |¡ƒtdtd
ƒt| ƒ d td
ƒ t| ƒ d gtdƒd	 tdƒdt| ƒ t| ƒ  d	 tdƒt| ƒ dt| ƒ   d	 gtdƒd	 tdƒdt| ƒ t| ƒ  d	 tdƒ t| ƒdt| ƒ   d	 ggƒ td
ƒk�sÚt‚| |¡|ˆj
 k�sôt‚ˆ j ˆj¡|j
|j k�st‚ˆ j |¡||j k�s0t‚ˆj ˆ¡tdƒk�sJt‚ˆ j ˆ¡||j k�sft‚tƒ \}}‰‰ tdƒ‰ˆ |dt d
 t td fd¡ t	dˆˆ | ||j ˆ j
ˆˆj
ˆj d�	 t| |¡ƒttdƒ t| ƒ d td
ƒ t| ƒd  d	 t| ƒd	  tdƒd	  td
ƒt| ƒd  d	 t| ƒd	  tdƒd	  gt| ƒtdƒtdƒ t| ƒ  d	 tdƒ tdƒ t| ƒ d	 gtdƒt| ƒ d td
ƒt| ƒd  d	 t| ƒd	  tdƒd	  td
ƒdt| ƒ  d	 t| ƒd	  tdƒd	  ggƒk�st‚| |¡tdƒ| d |j
 tdƒ| d |j  k�sBt‚ˆ j ˆj¡|j |j
 k�sdt‚ˆ j |¡ ¡ tdƒ tdƒ | d	 |j tdƒ tdƒ | d	 |j  k�sºt‚ˆ j ˆ¡tdƒk�sÔt‚tƒ \}}‰‰ tdƒtdƒ ‰‰ˆ |t d ttd fd¡ ˆ |dt d
 t td fd¡ t	dˆˆ | ||j
|j ˆ j ˆˆˆj
ˆj d�
 t| |¡ƒtt| ƒtdƒtdƒ t| ƒ  d	 tdƒ tdƒ t| ƒ d	 gtdƒ t| ƒ d td
ƒ t| ƒd  d	 t| ƒd	  tdƒd	  td
ƒt| ƒd  d	 t| ƒd	  tdƒd	  gtdƒt| ƒ d td
ƒt| ƒd  d	 t| ƒd	  tdƒd	  td
ƒ t| ƒd  d	 t| ƒd	  tdƒd	  ggƒk�s¦t‚| |¡tdƒ| d ˆj
 tdƒ| d ˆj  k�sât‚ˆ j ˆj¡|j
|j |j k�st‚tdƒ tdƒ | d	 |j
 }ˆ j |¡ ¡ |k�sBt‚ˆ j ˆ¡ ¡ |k�s\t‚ˆ j ˆ¡tdƒk�svt‚d S )Nr[   r*   r+   c                      s   t dˆˆ ˆd�S ©Nr2   ©rZ   ©r   r,   ©r$   r!   r*   r,   r-   r4   D  r5   z+test_pin_joint_interframe.<locals>.<lambda>c                      s   t dˆˆ ˆd�S ©Nr2   ©r]   rŠ   r,   ©r$   r+   r!   r,   r-   r4   E  r5   c                      s   t dˆˆ ˆd�S rˆ   rŠ   r,   r‹   r,   r-   r4   I  r5   c                      s   t dˆˆ ˆd�S rŒ   rŠ   r,   rŽ   r,   r-   r4   J  r5   r   r:   Zxyzr2   )rW   rX   rZ   rY   g      à¿r&   é   éþÿÿÿr   )rW   rX   r]   rY   r8   r\   )r   r.   r   r   rH   r'   re   Zorient_body_fixedr   r   r(   r)   r	   rs   r   r   r   r   r   rG   r}   rb   ra   rg   r   r   )r<   r;   r   r    ZN_v_Cr,   ©r$   r+   r!   r*   r-   Útest_pin_joint_interframe?  sÖ     ÿ*( ÿ( ÿüû
 " 
ÿ0.þ"ÿ..þú	<" ÿÿÿ ÿÿÿÿ" 
þ"ÿ0.þ.0þú	ÿÿÿÿ& r’   c               
      sú  t dƒ\‰} tdƒ\‰}‰‰ ‰‰tdˆˆ ˆ| ˆˆˆjd�}|jˆjksJt‚ˆ |¡ttˆƒdt	ˆƒgdddgt	ˆƒdtˆƒ ggƒksŠt‚tdƒ\‰}‰‰ ‰‰tdˆˆ ˆ| ˆˆˆjd�}|jˆjksÈt‚ˆ |¡ttˆƒ dt	ˆƒgdddgt	ˆƒdtˆƒggƒk�s
t‚tƒ \‰}‰‰ tdˆˆ ˆ| ˆj
ˆ j
 ˆj
d�}|jˆj
k�sJt‚ˆ |¡tt	ˆƒ tˆƒ dgtˆƒ t	ˆƒdgdddggƒk�s�t‚tƒ \‰}‰‰ tdˆˆ ˆ| ˆj
ˆ j
 ˆjd�}|jˆjk�sÐt‚ˆ |¡tdddgdt	ˆƒtˆƒgdtˆƒt	ˆƒ ggƒk�st‚tdƒ\‰}‰‰ ‰‰tt‡ ‡‡‡fdd„ƒ tt‡ ‡fd	d„ƒ tt‡ ‡fd
d„ƒ tt‡ ‡‡‡fdd„ƒ tt‡ ‡‡‡fdd„ƒ tdƒ\‰}‰‰ ‰‰tdˆˆ ˆˆˆjˆj d� tt‡ ‡‡‡fdd„ƒ tt‡ ‡‡‡fdd„ƒ d S )Nr[   Tr2   ©rZ   r]   rY   r   éÿÿÿÿc                      s(   t dˆˆ tˆƒˆj tˆƒˆj  d�S ©Nr2   )rY   )r   r   r(   r   r)   r,   ©r$   r   r!   r<   r,   r-   r4   ª  s   ÿz+test_pin_joint_joint_axis.<locals>.<lambda>c                      s   t dˆˆ ˆ jd�S r•   ©r   r(   r,   ©r$   r!   r,   r-   r4   ­  r5   c                      s   t dˆˆ ˆjˆ j d�S r•   ©r   r(   r)   r,   r˜   r,   r-   r4   ¯  r5   c                      s   t dˆˆ ˆˆˆjˆ j d�S ©Nr2   r“   r™   r,   r‘   r,   r-   r4   °  s       
þc                      s   t dˆˆ ˆˆˆjˆj d�S rš   r™   r,   r‘   r,   r-   r4   ³  s       
þc                      s   t dˆˆ ˆˆtdƒd�S )Nr2   r   r“   )r   r   r,   r‘   r,   r-   r4   »  s       þc                      s   t dˆˆ ˆˆˆjˆj d�S rš   )r   r)   r,   r‘   r,   r-   r4   ¾  s       
þ)r   r.   r   r)   rY   rG   rs   r   r   r   re   r(   r   rH   Ú	Exception)r;   r    Úpinr,   )r$   r+   r   r!   r*   r<   r-   Útest_pin_joint_joint_axis‹  sb     ÿ"ÿ ÿ$ÿ ÿ0ÿ ÿ"ÿ
ÿr�   c            	      C   sV  t dƒ\} }tƒ \}}}}td|||j d� |j  |j¡dksFt‚|j |¡ |jks^t‚| |¡tdddgdt	| ƒ t
| ƒ gdt
| ƒ t	| ƒggƒks¢t‚| |¡||j ksºt‚| |¡ ¡ t|d ƒksØt‚|j |j¡dksît‚|j |j¡ |¡ ¡ dk�st‚|j |¡dk�s&t‚tƒ \}}}}td|||j|jd� |j |j¡dk�sbt‚|j |¡|jk�szt‚| |¡tdt	| ƒ t
| ƒ gdddgdt
| ƒ t	| ƒggƒk�sÀt‚| |¡||j k�sÚt‚| |¡ |¡||j k�sút‚| |¡ ¡ t|d ƒk�st‚| |¡ |j¡dk�s6t‚|j |¡||j k�sRt‚|j |j¡|j k�snt‚|j |j¡ |¡ ¡ t	| ƒ|j t
| ƒ|j  k�s¨t‚|j |¡ |j¡td k�sÊt‚tƒ \}}}}td|||j|jd	� |j |j¡dk�st‚|j |¡|jk�st‚| |¡tdddgt	| ƒ dt
| ƒgt
| ƒdt	| ƒggƒk�s`t‚| |¡||j k�szt‚| |¡ |¡||j k�sšt‚| |¡ ¡ t|d ƒk�sºt‚| |¡ |j¡}| |di¡dk�sät‚|j |¡dk�sút‚|j |j¡|jk�st‚tƒ \}}}}td|||j|j|j|j d
� t|j |j|j ¡ƒdk�sdt‚|j|j  |¡ ¡ tdƒ|j k�sŽt‚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 t‚| |¡||j k�s:t‚| |¡ |¡ ¡ ||j ||j  tdƒ k�spt‚| |¡ ¡ t|d ƒk�s�t‚| |¡ |j|j ¡}| |di¡dk�sÀt‚|j |¡ ¡ ||j tdƒ k�sèt‚|j |j¡|j|j k�st‚|j |j¡ |¡ ¡ dtdƒd  |j tdƒt	| ƒ d |j  tdƒt
| ƒ d |j  k�spt‚|j |¡ |¡ ¡ tdƒ | t
| ƒ d |j tdƒ| t	| ƒ d |j  k�sÊt‚|j |¡ |j¡td k�sìt‚tƒ \}}}}td|||j|j|j|j |j d
� t|j |j|j |j ¡ƒdk�sHt‚|j|j |j  |¡ ¡ tdƒ|j k�sxt‚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`t‚| |¡||j k�szt‚| |¡ |¡ ¡ ||j ||j  ||j  tdƒ k�sºt‚| |¡ ¡ t|d ƒk�sÚt‚| |¡ |j|j |j ¡}| |di¡ ¡ dk�	st‚|j |¡ ¡ ||j ||j  tdƒ k�	sFt‚|j |j¡|j|j k�	sft‚|j |j¡ |¡ ¡ dtdƒd  |j tdƒt
| td  ƒ d |j  tdƒt	| td  ƒ d |j  k�	sÞt‚|j |¡ |¡ ¡ tdƒ| t	| td  ƒ d |j tdƒ| t
| td  ƒ d |j  k�
sFt‚|j |¡ |j¡td k�
sht‚tƒ \}}}}tdƒ\}}td||||j ||j |j|j |j |j|j |j d� |j|j |j  |j|j |j ¡}t|ƒdk�
søt‚|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�t‚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�sÌt‚| |¡||j ||j  ||j  tdƒ   ¡ �st‚| |¡ |¡ ¡ ||j ||j  ||j  tdƒ k�sFt‚| |¡ ¡ t|d ƒk�sft‚| |¡ |j|j |j ¡}| |di¡ ¡ dk�s t‚|j |¡ ¡ tdƒ| | d |j tdƒ| | d |j  k�sêt‚|j |j¡||j ||j  k�st‚|j |j¡ |¡ ¡ ||dt	| ƒ d  d  |j |dt
| td  ƒ d  d |j  |dt	| td  ƒ d  d |j  k�sžt‚|j |¡ |¡ ¡ d| | t
| ƒ d |j d| | t	| td  ƒ d |j  d| | t
| td  ƒ d |j  k�s$t‚|j |¡ |j|j |j ¡ ¡ dk�sRt‚d S )NrR   r2   r�   r   r”   r&   rn   r8   ©rZ   rW   ©rW   rX   r]   r:   r�   r   é   é   úm n©rW   rX   r]   rZ   éüÿÿÿr�   )r   r.   r   r(   Úangle_betweenrG   rh   rs   r   r   r   r}   Ú	magnituder   rb   ra   r	   rg   r)   Úcrossre   r   rt   r   r   r   Údot)	r<   r;   r   r    r!   r$   Zanglerj   Únr,   r,   r-   Útest_pin_joint_arbitrary_axisÃ  sR   þ"þ  ÿ
"þ  
ÿ"*40ýÿ
 ( .ÿÿ
<ÿ
"ÿ(0&ÿ$ÿ*ÿû$ÿÿ 2 6 ÿÿ
$$ÿÿ
"þ&J(ÿÿ
<"ÿ<"ÿDÿû:$ÿÿ 2ÿ
(4ÿÿÿ$ÿÿ
B$ÿÿ
rª   c                  C   sZ  t ƒ \} }}}t ||j |j¡}|j|jks2t‚t ||j |j¡}|j|jksVt‚t ||j |j¡}|j|jkszt‚t ||j |j |j|j ¡}|j|jksªt‚t ||j |j |j|j ¡}|j|jksÚt‚t ||j |j |j|j ¡}|j|jk�st‚t ||j |j |j |j|j |j ¡}|j|j |j|j k�sVt‚d S ©N)r.   r   Z_create_aligned_interframer(   re   rG   r)   )r   r    r!   r$   Úfr,   r,   r-   Útest_create_aligned_frame_piR  s       ,r­   c               
      s*  t dƒ\‰} tdƒ\‰}‰‰ }}tdˆˆ ˆ| ||d�}|j|jksFt‚tdtˆƒtˆƒgdtˆƒ tˆƒgdddggƒ}tdƒ\‰}‰‰ }}tdˆˆ ˆ| ||ˆj	d� ˆ 
|¡|ks¸t‚tdƒ\‰}‰‰ }}tdˆˆ ˆ| |||j	 d� ˆ 
|¡|ksút‚tdƒ\‰}‰‰ }}tt‡ ‡‡‡fdd	„ƒ d S )
Nzq uTr2   )rZ   r]   r   r8   r“   c                      s   t dˆˆ ˆˆj d�S r•   )r   re   r,   r–   r,   r-   r4   x  r5   z%test_pin_joint_axis.<locals>.<lambda>)r   r.   r   rY   r(   rG   r   r   r   re   rs   r   rH   )r;   r    r*   r+   r2   ÚN_R_Ar,   r–   r-   Útest_pin_joint_axisd  s(    þÿÿr¯   c                     sf  t ƒ \‰} ‰‰ tdˆˆ ˆjˆj d�}|jjdks6t‚|j ˆj¡ˆjˆj ksTt‚|j	ˆ jksdt‚t ƒ \‰} ‰‰ ˆj 
dˆjˆj ¡}tdˆˆ |ˆ jd�}|j|ksªt‚|j	ˆ jksºt‚t ƒ \‰} ‰‰ tt‡ ‡‡fdd„ƒ tdƒ‰t ƒ \‰} ‰‰ tt‡ ‡‡‡fd	d„ƒ t ƒ \‰} ‰‰ ˆ j 
dˆ| j ¡‰tt‡ ‡‡fd
d„ƒ tdƒ‰tt‡ ‡‡fdd„ƒ d S )Nr2   ©rW   Z	J_P_jointÚp)rW   rX   c                      s   t dˆˆ ˆj ˆ¡d�S ©Nr2   r°   )r   r(   Ú	to_matrixr,   )r$   r   r!   r,   r-   r4   Œ  r5   z'test_locate_joint_pos.<locals>.<lambda>r<   c                      s   t dˆˆ ˆˆj d�S r²   r—   r,   r–   r,   r-   r4   �  r5   c                      s   t dˆˆ ˆd�S ©Nr2   rw   rŠ   r,   ©r$   r!   rX   r,   r-   r4   “  r5   c                      s   t dˆˆ ˆd�S r´   rŠ   r,   rµ   r,   r-   r4   –  r5   )r.   r   r)   re   rW   r^   rG   ra   rb   rX   rf   r   r6   r   rH   r   )r    ÚjointrW   r,   )r$   r   r!   rX   r<   r-   Útest_locate_joint_pos{  s0    
ÿÿr·   c                     sê   t ƒ \} }‰‰ tdƒ‰ˆ | | jd¡ tdˆˆ ˆd�}|jˆksDt‚|j | ¡dksXt‚|j|ksft‚t	dƒ}t ƒ \} }‰‰ tdƒ‰ˆ | | j|¡ t
t‡ ‡‡fdd„ƒ t ƒ \} }‰‰ tdƒ‰ˆ | | jd¡ t
t‡ ‡‡fd	d„ƒ d S )
NZ	int_framer8   r2   r‰   r   r<   c                      s   t dˆˆ ˆd�S rˆ   rŠ   r,   )r$   r!   rZ   r,   r-   r4   ¨  r5   z)test_locate_joint_frame.<locals>.<lambda>c                      s   t dˆˆ ˆd�S rŒ   rŠ   r,   )r$   r!   r]   r,   r-   r4   ®  r5   )r.   r   r'   re   r   rZ   rG   r}   r]   r   r   rH   )r   r    r¶   r<   r,   )r$   r!   r]   rZ   r-   Útest_locate_joint_frame™  s(    ÿÿr¸   c               	   C   sH  t ƒ \} } }}tdƒ\}}td||ƒ}|jdks4t‚|j|ksBt‚|j|ksPt‚|jt|gƒksdt‚|j	t|gƒksxt‚|j
t|| t¡ gƒks–t‚|j|jjks¨t‚|j |j¡tdƒksÂt‚|j |j¡tdƒksÜt‚|j |j¡| |jj ksüt‚|j |j¡| |jj k�st‚|j |j¡||jj k�s>t‚|j |j¡dk�sVt‚|j |j¡dk�snt‚| ¡ dk�s€t‚t ƒ \}}}}tdƒ\}}	tdƒ}
|
 |j|jtd ¡ td||||jj |	|jj |jj|
d�}|j|jjk�söt‚|j |j¡|	|jj k�st‚|j |j¡||jj k�s6t‚|j |j¡| |jj k�sXt‚|j |j¡| |j ||j  |	|j  k�sŒt‚|j |j¡||jj k�s¬t‚|j |
¡tdƒk�sÆt‚|j |j¡dk�sÞt‚|j |j¡dk�söt‚t ƒ \} } }}tdƒ}
|
 |j|jtd ¡ td||||jj |	|jj |jj|
d�}|j|jjk�s`t‚|j |j¡|	|jj k�s€t‚|j |j¡||jj k�s t‚|j |j¡| |jj k�sÂt‚|j |j¡| | |jj |	|jj  k�sôt‚|j |j¡||jj k�st‚|j |j¡dk�s,t‚|j |j¡dk�sDt‚d S )	Núq_S, u_Sr   r   z&PrismaticJoint: S  parent: P  child: CrQ   r%   r&   rV   )r.   r   r   r^   rG   r/   r0   r_   r   rF   r`   rI   rJ   rY   r#   r(   rX   ra   rb   r   rW   rg   r}   rd   r   r   r'   r)   r   re   )rO   r!   r$   r<   r;   r   r   r    ri   rj   r*   r,   r,   r-   Útest_prismatic_joint±  sh     " 
 þ  "4 
 þ  "2 rº   c               	   C   sÀ
  t dƒ\} }tƒ \}}}}td|||j d� |j  |j¡dksFt‚|j |¡ |jks^t‚| |¡tdddgdddgdddggƒksŒt‚|j	 
|j	¡| |j ks¨t‚|j	 
|j	¡ |¡ ¡ |  |j ksÐt‚|j	 |¡||j ksêt‚|j	 |¡ |¡| |j k�st‚| |¡dk�s"t‚| |¡dk�s6t‚tƒ \}}}}td|||j|jd� |j |j¡dk�srt‚|j |¡|jk�sŠt‚| |¡tdddgdddgdddggƒk�sºt‚|j	 |¡||j k�sÖt‚|j	 |¡ |¡||j k�søt‚|j	 
|j	¡| |j |j k�st‚|j	 
|j	¡ |¡ ¡ | |j |j k�sJt‚| |¡dk�s^t‚| |¡dk�srt‚tƒ \}}}}td|||j|jd� |j |j¡dk�s®t‚|j |¡|jk�sÆt‚| |¡tdddgdddgdddggƒk�söt‚|j	 |¡||j k�st‚|j	 |¡ |¡||j k�s4t‚|j	 
|j	¡|j| |j  k�sXt‚| |¡dk�slt‚| |¡dk�s€t‚tƒ \}}}}td|||j|j|j|j d	� |j |j|j ¡dk�sÌt‚|j|j  |¡td
ƒ|j k�sòt‚| |¡ttd
ƒd
 td
ƒ d
 dgtd
ƒd
 td
ƒd
 dgdddggƒk�sDt‚|j	 
|j	¡| d |j |j k�slt‚|j	 
|j	¡ |¡| td
ƒd
  d |j td
ƒd
 |j  k�s²t‚|j	 |¡ |¡||j|j  td
ƒ k�sât‚|j	 |¡||j k�sþt‚| |¡dk�st‚| |¡dk�s&t‚tƒ \}}}}td|||j|j|j|j |j d	� |j |j|j |j ¡ ¡ dk�s‚t‚|j|j |j  |¡tdƒ|j   ¡ dk�s¶t‚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�sft‚|j	 
|j	¡| d |j |j k�sŽt‚|j	 
|j	¡ |¡| tdƒd  d |j tdƒd |j  tdƒd |j    ¡ dk�sît‚|j	 |¡||j k�s
t‚|j	 |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j    ¡ �sht‚| |¡dk�s|t‚| |¡dk�s�t‚tƒ \}}}}tdƒ\}}td||||j ||j |j|j |j |j|j |j d� |j|j |j  |j|j |j ¡ ¡ dk�st‚|j|j |j  |¡|j|j |j   ¡ dk�sTt‚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�sÖt‚|j	 
|j	¡|tdƒ|  d  |j tdƒ|  d |j  tdƒ|  d |j  ||j    |¡ ¡ dk�	sHt‚|j	 
|j	¡ |¡||d  tdƒ|  d  |j d
| d tdƒ|  d  |j  d| d tdƒ|  d  |j    ¡ dk�	sÐt‚|j	 |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j    ¡ dk�
s2t‚|j	 |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j    ¡ dk�
s”t‚| |¡dk�
s¨t‚| |¡dk�
s¼t‚d S )Nr¹   r   r�   r   r”   r8   rn   rž   rŸ   r&   r:   r�   r¢   r£   r�   )r   r.   r   r(   r¥   rG   rh   rs   r   rb   ra   r	   rg   r}   r)   r   re   r   r   )r<   r;   r   r    r!   r$   rj   r©   r,   r,   r-   Ú#test_prismatic_joint_arbitrary_axisç  sÔ    .($0"$.0"$
ÿ&R(F0ÿ(408:þ(<ÿÿ
@ÿþ482 "þNÿþþ
B ÿÿþ
@ÿÿ
@ÿÿ
r»   c                  C   s¸  t ƒ \} }}}tdƒ\}}}}td||ƒ}|jdks8t‚|j|ksFt‚|j|ksTt‚|jt||gƒksjt‚|j	t||gƒks€t‚|j
|ksŽt‚|j|ksœt‚|j|ksªt‚|j|ks¸t‚|jt|| t¡ || t¡ gƒksât‚|j| jksòt‚|j |j¡tdƒk�st‚|j |j¡tdƒk�s*t‚|j |j¡| | j k�sJt‚|j |j¡|| j k�sht‚|j | ¡|| j k�s„t‚| | ¡|| j k�sžt‚|j| k�s®t‚|j|k�s¾t‚| ¡ dk�sÐt‚tdƒ\}	}
}}tdƒ\}}t dƒ\} }}}}}td|||	|||| j ||j ||d| j  d	�}|jt|	|gƒk�sHt‚|j	t||gƒk�s`t‚|j
|	k�spt‚|j|k�s€t‚|j|k�s�t‚|j|k�s t‚|jt||	 t¡ || t¡ gƒk�sÌt‚|jd| j  k�sât‚|j |j¡||j k�s t‚|j |j¡|| j k�st‚|j |j¡| | j  k�s>t‚|j |j¡|| j || j   ||j  k�spt‚|j | ¡|| j  || |j   k�sšt‚| | ¡|| j  k�s´t‚d S )
Nzq0:2_J, u0:2_Jr2   r   z(CylindricalJoint: J  parent: P  child: Cz
q0:2, u0:2úl, mTr&   )Úrotation_coordinateÚrotation_speedÚtranslation_speedrW   rX   rZ   r]   rY   )!r.   r   r   r^   rG   r/   r0   r_   r   rF   r½   Ztranslation_coordinater¾   r¿   r`   rI   rJ   rY   r(   rX   ra   rb   r   rW   rc   rg   r}   rZ   r]   rd   r   r)   re   )r   r    r!   r$   Úq0_defÚq1_defÚu0_defÚu1_defÚCjr@   rA   rB   rC   ri   rj   r*   r+   r,   r,   r-   Útest_cylindrical_jointK  sh    *    ý, ÿÿ
*rÅ   c                  C   sh  t ƒ \} }}}tdƒ\}}}tdƒ\}}}	td||ƒ}
|
jdksDt‚|
j|ksRt‚|
j|ks`t‚|
jt|||gƒksxt‚|
j	t|||	gƒks�t‚|
j
|ksžt‚|
jt||gƒks´t‚|
j|ksÂt‚|
jt||	gƒksØt‚|
jt|| t¡ || t¡ |	| t¡ gƒk�st‚|
j| jk�s"t‚|
j| j| jgk�s:t‚|
j |j¡tdƒk�sVt‚|
j |j¡tdƒk�srt‚|| j || j  }|
j |
j¡| k�s t‚|j |j¡|k�s¸t‚|
j | ¡|| j |	| j  k�sÞt‚| | ¡|| j k�søt‚|
j| k�st‚|
j|k�st‚|
 ¡ dk�s*t‚tdƒ\}}}}}}t dƒ\}}t dƒ\} }}}}}td|||||g||g|| j ||j ||d	�
}
|
jt|||gƒk�s¨t‚|
j	t|||gƒk�sÂt‚|
j
|k�sÒt‚|
jt||gƒk�sêt‚|
j|k�sút‚|
jt||gƒk�st‚|
jt|| t¡ || t¡ || t¡ gƒk�sJt‚|
j|jk�s\t‚|
j|j|jgk�stt‚|
j |j¡||j k�s’t‚|
j |j¡|| j k�s°t‚|
j |
j¡|| j || j  k�sØt‚|j |j¡|| j || j  || j  ||j  k�st‚|j | ¡| | j || j  || |j  k�sJt‚| | ¡|| j k�sdt‚d S )
Nzq0:3_Jzu0:3_Jr2   r   z#PlanarJoint: J  parent: P  child: Cú
q0:3, u0:3r¼   T)r½   Úplanar_coordinatesÚplanar_speedsrW   rX   rZ   r]   )!r.   r   r   r^   rG   r/   r0   r_   r   rF   r½   rÇ   r¾   rÈ   r`   rI   rJ   Úrotation_axisr(   Zplanar_vectorsr)   re   rX   ra   rb   r   rW   rg   r}   rZ   r]   rd   r   )r   r    r!   r$   rÀ   rÁ   Zq2_defrÂ   rÃ   Zu2_defrÄ   Zr_P_Cr@   rA   rK   rB   rC   rM   ri   rj   r*   r+   r,   r,   r-   Útest_planar_joint|  sx    ÿ&
   ýÿ(ÿ&ÿ
6rÊ   c                  C   sÖ  t dƒ\} }}}}}tdƒ\}}tƒ \}}	}
}td|
|| ||g|||g||j | |j |j|jtdƒ  |	j|	jtdƒ  d�}|j |¡|j|jtdƒ   	¡ ksªt
‚|j |	¡|	j|	jtdƒ   	¡ ksÒt
‚|j |j¡td ksìt
‚| |	¡ | d|d|di¡tdƒk�st
‚tt| ƒtdƒ t| ƒ d t| ƒd gtdƒt| ƒ d dt| ƒ d	 d
 tdƒdt| ƒ  d	 gt| ƒ d tdƒdt| ƒ  d	 t| ƒd	 d ggƒ}t| |	¡| ƒtdƒk�sÒt
‚d S )NrÆ   zl1:3r2   r:   )rW   rX   rZ   r]   r�   r   r&   r   g      Ð?r8   g      è?)r   r   r.   r   re   r)   r   rÉ   rh   Ú	normalizerG   r¥   r   rs   rt   r
   r   r   r   r	   r   )r@   rA   rK   rB   rC   rM   Úl1Úl2r   r    r!   r$   r2   r®   r,   r,   r-   Útest_planar_joint_advanced³  s*    
ü((*&$ÿ2ürÎ   c                  C   st  t ƒ \} }}}tdƒ\}}}}}}	td||ƒ}
|
jdks<t‚|
j|ksJt‚|
j|ksXt‚|
jt|||gƒkspt‚|
j	t|||	gƒksˆt‚|
j
t|| t¡ || t¡ |	| t¡ gƒks¾t‚|
j |j¡tdƒksØt‚|
j |j¡tdƒksòt‚|
j |
j¡tdƒk�st‚|j |j¡tdƒk�s*t‚|j | ¡tdƒk�sDt‚|  |¡| t|ƒ t|ƒ |t|ƒ  |j |t|ƒ t|ƒ |t|ƒ  |j  | t|ƒ |	 |j  k�sºt‚| | ¡|t|ƒ t|ƒ |t|ƒ  |j | t|ƒ t|ƒ |t|ƒ  |j  |t|ƒ |	 |j  k�s.t‚|
 ¡ dk�s@t‚|
jdk�sPt‚|
jdk�s`t‚|
jd k�spt‚d S )Núq0:3_S, u0:3_Sr   r   z&SphericalJoint: S  parent: P  child: CÚBODYé{   )r.   r   r   r^   rG   r/   r0   r_   r   rF   r`   rI   rJ   rX   ra   rb   r   rW   rg   r}   r   r   r(   r)   re   rd   Ú	_rot_typeÚ
_rot_orderZ_amounts)r   r    r!   r$   r@   rA   rK   rB   rC   rM   r   r,   r,   r-   Útest_spherical_jointÎ  sD    6. ÿ ÿÿ,  ÿ ÿÿrÔ   c                  C   s  t dƒ\} }}t ddƒ\}}}tƒ \}}}}	td||	| ||g|||gd�}
|
jt| ||gƒksbt‚|
jt|||gƒkszt‚|
jtdddgƒks’t‚| |¡| t	|ƒ t	|ƒ |t
|ƒ  |j |t
|ƒ t	|ƒ |t	|ƒ  |j  | t
|ƒ | |j  k�st‚d S )Nzq0:3r8   r   )r_   rF   r   )r   r.   r   r_   r   rG   rF   r`   r}   r   r   r(   r)   re   )r@   rA   rK   rB   rC   rM   r   r    r!   r$   r   r,   r,   r-   Ú/test_spherical_joint_speeds_as_derivative_termsé  s    . ÿ ÿÿrÕ   c            	         sX  t dƒ\} }}}}}t dƒ\‰‰‰‰‰‰‰‰	tƒ \}}‰‰ tdˆˆ ˆˆˆgˆˆˆgƒ}|jtˆˆˆgƒksnt‚|jtˆˆˆgƒks†t‚tƒ \}}‰‰ tdˆˆ tˆˆˆgƒtˆˆˆgƒƒ}|jtˆˆˆgƒksÐt‚|jtˆˆˆgƒksèt‚tƒ \}}‰‰ tt‡ ‡‡‡‡fdd„ƒ tt‡ ‡‡‡‡‡‡‡‡f	dd„ƒ tt‡ ‡‡‡‡‡‡‡‡	f	dd„ƒ d S )NrÏ   z
q0:4, u0:4r   c                      s   t dˆˆ tˆˆgƒtˆgƒƒS ©Nr   ©r   r   r,   )r$   r!   r@   rA   rB   r,   r-   r4   	  r5   z-test_spherical_joint_coords.<locals>.<lambda>c                	      s&   t dˆˆ tˆˆˆˆgƒtˆˆˆgƒƒS rÖ   r×   r,   )	r$   r!   r@   rA   rK   rL   rB   rC   rM   r,   r-   r4     s       ÿc                
      s&   t dˆˆ tˆˆˆgƒtˆˆˆˆgƒƒS rÖ   r×   r,   )	r$   r!   r@   rA   rK   rB   rC   rM   Úu4r,   r-   r4     s       ÿ)	r   r.   r   r_   r   rG   rF   r   rH   )	Zq0sZq1sZq2sZu0sZu1sZu2sr   r    r   r,   )
r$   r!   r@   rA   rK   rL   rB   rC   rM   rØ   r-   Útest_spherical_joint_coordsø  s$    ÿÿ"rÙ   c                     s$  t dƒ\‰ ‰‰‰‰‰ttˆƒ tˆƒ tˆƒ tˆƒtˆƒ gtˆ ƒ tˆƒ tˆ ƒtˆƒ tˆƒ tˆ ƒtˆƒ  tˆ ƒ tˆƒ tˆƒ tˆƒtˆ ƒ  gtˆ ƒtˆƒ tˆ ƒ tˆƒ tˆƒtˆƒ tˆ ƒ  tˆ ƒ tˆƒ tˆƒtˆ ƒ tˆƒ  ggƒ} tˆ tˆƒ ˆ gˆ tˆƒ tˆƒ ˆtˆƒ  gˆtˆƒ tˆƒ ˆtˆƒ  ggƒ}ttdƒ ˆtˆtd  ƒ tˆƒ ˆtˆtd  ƒ   gˆ tˆƒ ˆ gˆ tˆƒ ˆ ggƒ}tdƒ\}}}}}}td||ˆ ˆˆgˆˆˆg|j|j	 |j	 |j
 ||ddd�}	|	j ¡ d	k�s*t‚|	jdk�s:t‚t| |¡|  ƒtd
ƒk�sZt‚t| |¡ |¡| ƒtd
dƒk�s‚t‚t|j |¡ |¡ƒ|k�s¢t‚tdƒ\}}}}}}td||ˆ ˆˆgˆˆˆg|j|j	 |j	 |j
 ||d	ˆˆ ˆfdd�}	‡ ‡‡‡‡‡fdd„}
|	j ¡ d	k�s$t‚|	jdk�s4t‚t| |¡|
| ƒ ƒtd
ƒk�sXt‚t| |¡ |¡|
|ƒ ƒtd
dƒk�s„t‚t|j |¡ |¡ƒ|
|ƒk�s¨t‚tdƒ\}}}}}}td||ˆ ˆˆgˆˆˆg|j|j	 |j	 |j
 ||ddd�}	|	j ¡ d	k�st‚|	jdk�st‚t| |¡ttˆ ƒ tˆƒ tˆ ƒtˆƒ tˆƒ tˆƒtˆ ƒ  tˆ ƒtˆƒ tˆƒ tˆ ƒtˆƒ  gtˆƒ tˆƒ tˆƒ tˆƒ tˆƒ gtˆ ƒtˆƒ tˆ ƒ tˆƒ tˆƒtˆ ƒ tˆƒ  tˆ ƒtˆƒ tˆƒtˆƒ tˆ ƒ  ggƒ ƒtd
ƒk�s&t‚t| |¡ |¡tˆtˆƒ ˆ gˆtˆƒ tˆƒ ˆtˆƒ  gˆtˆƒ tˆƒ ˆtˆƒ  ggƒ ƒtd
dƒk�s¢t‚t|j |¡ |¡ƒttdƒ ˆtˆtd  ƒ tˆƒ ˆtˆtd  ƒ   gˆtˆƒ ˆ gˆtˆƒ ˆ ggƒk�s t‚d S )NrÆ   r&   r   Tr   ÚbodyrÑ   ©r_   rF   rW   rX   rZ   r]   Úrot_typeÚ	rot_orderrÐ   r:   r8   ©	r_   rF   rW   rX   rZ   r]   rÜ   ZamountsrÝ   c                    s"   |   ˆ ˆˆˆ ˆˆˆˆˆˆˆˆi¡S r«   ©rt   ©Úexpr©r@   rA   rK   rB   rC   rM   r,   r-   r4   0  s   ÿz2test_spherical_joint_orient_body.<locals>.<lambda>ZBodYZyxz)r   r   r   r   r   r   r.   r   r(   r)   re   rÒ   ÚupperrG   rÓ   r	   rs   r   r}   r³   rb   rg   ©r®   ZN_w_AZN_v_Cor   r    r!   r$   r*   r+   r   Zswitch_orderr,   râ   r-   Ú test_spherical_joint_orient_body  sª    (6(ÿ6(ÿü" þ: þ
   ý ( 
    ý$ÿ
$
   ý6&ÿ*6&ÿüû
   þþ
: þrå   c                     s&  t dƒ\‰ ‰‰‰‰‰ttˆ ƒ tˆƒ tˆƒtˆ ƒ tˆƒ  tˆ ƒtˆƒ tˆƒ tˆƒtˆ ƒ  tˆƒtˆƒ gtˆ ƒ tˆƒ tˆƒtˆƒ tˆ ƒ  tˆ ƒ tˆƒ tˆƒ tˆ ƒtˆƒ  tˆƒ tˆƒ gtˆ ƒtˆƒ tˆ ƒ tˆƒ tˆƒggƒ} tˆtˆ ƒ ˆtˆ ƒ tˆƒ  gˆtˆ ƒ ˆtˆ ƒ tˆƒ  gˆˆtˆƒ  ggƒ}tˆˆtˆƒ  gˆˆtˆƒ  gtdƒˆ tˆ td  ƒ ˆtˆ td  ƒ tˆƒ   ggƒ}tdƒ\}}}}}}td||ˆ ˆˆgˆˆˆg|j|j	 |j |j
 ||ddd�}	|	j ¡ d	k�s t‚|	jdk�s0t‚t| |¡|  ƒtd
ƒk�sPt‚t| |¡ |¡ƒ|k�snt‚t|j |¡ |¡ƒ|k�sŽt‚‡ ‡‡‡‡‡fdd„}
tdƒ\}}}}}}td||ˆ ˆˆgˆˆˆg|j|j	 |j |j
 ||d	ˆˆ ˆfdd�}	|	j ¡ d	k�st‚|	jdk�s t‚t| |¡|
| ƒ ƒtd
ƒk�sDt‚t| |¡ |¡ƒ|
|ƒk�sft‚t|j |¡ |¡ƒ|
|ƒk�sŠt‚tdƒ\}}}}}}td||ˆ ˆˆgˆˆˆg|j|j	 |j |j
 ||ddd�}	|	j ¡ d	k�sît‚|	jdk�sþt‚t| |¡ttˆƒ tˆƒ tˆ ƒ tˆƒ tˆƒtˆƒ tˆ ƒ  tˆ ƒtˆƒ tˆƒ tˆ ƒtˆƒ  gtˆƒ tˆ ƒ tˆƒ tˆ ƒ tˆƒ gtˆƒtˆƒ tˆ ƒ tˆƒ tˆƒtˆ ƒ tˆƒ  tˆ ƒ tˆƒ tˆƒ tˆƒtˆ ƒ  ggƒ ƒ�st‚t| |¡ |¡tˆ ˆtˆƒ  gˆ tˆ ƒ ˆtˆ ƒ tˆƒ  gˆtˆ ƒ ˆtˆ ƒ tˆƒ  ggƒ ƒtd
dƒk�s„t‚t|j |¡ |¡tˆtˆ ƒ ˆtˆ ƒ tˆƒ  gˆtˆ ƒ ˆtˆ ƒ tˆƒ  gˆˆtˆ ƒ  ˆtˆƒ  ˆtˆ ƒ tˆƒ  ggƒ ƒtd
dƒk�s"t‚d S )NrÆ   r&   r   Tr   ÚspacerÑ   rÛ   ÚSPACEr:   c                    s"   |   ˆ ˆˆˆ ˆˆˆˆˆˆˆˆi¡S r«   rß   rà   râ   r,   r-   r4   h  s   ÿz3test_spherical_joint_orient_space.<locals>.<lambda>rÞ   ZSPaCeZzxyr8   )r   r   r   r   r   r   r.   r   r(   re   r)   rÒ   rã   rG   rÓ   r	   rs   r   r}   r³   rb   rg   rä   r,   râ   r-   Ú!test_spherical_joint_orient_spaceN  s°    (& ÿ(( ÿ&û   þ :þ
   ý  
    ý$"$
   ý8&ÿ*6(ÿü " þþ
  ÿýürè   c            	   	   C   sÖ  t ƒ \} } }}td||ƒ}|jdks(t‚|j|ks6t‚|j|ksDt‚|jtƒ ksTt‚|jtƒ ksdt‚|j	tddg ƒj
ks|t‚|j |j¡tdƒks–t‚|j |j¡tdƒks°t‚|j |j¡tdƒksÊt‚|j |j¡tdƒksät‚|j |j¡tdƒksþt‚|j |j¡tdƒk�st‚|j |j¡dk�s2t‚|j |j¡dk�sJt‚| ¡ dk�s\t‚t ƒ \}}}}tdƒ\}}tdƒ}| |j|jtd ¡ td||||jj ||jj |d	�}|j |j¡||jj k�sØt‚|j |j¡||jj k�søt‚|j |j¡tdƒk�st‚|j |j¡| |j ||j  k�s>t‚|j |j¡tdƒk�sZt‚|j |¡tdƒk�stt‚|j |j¡dk�sŒt‚|j |j¡dk�s¤t‚|j|jk�s¶t‚tƒ � t||ƒ W 5 Q R X d S )
NÚWr8   r   r:   z!WeldJoint: W  parent: P  child: CrQ   r%   r&   )rW   rX   rZ   )r.   r   r^   rG   r/   r0   r_   r   rF   r`   ÚTr#   rs   r
   rX   ra   rb   r   rW   rg   r}   rd   r   r   r'   r)   r   r(   re   r   r   )	rO   r!   r$   ré   r   r    ri   rj   r*   r,   r,   r-   Útest_weld_jointŒ  sH    
 ÿ  *rë   c               	   C   sÔ  t dƒ\} }tƒ \}}}}tƒ � td|||j d� W 5 Q R X |j  |j¡dksXt‚|j |¡ |jkspt‚| |¡t	dddgdt
| ƒ t| ƒ gdt| ƒ t
| ƒggƒks´t‚| |¡||j ksÌt‚| |¡ ¡ t|d ƒksêt‚tƒ \}}}}tƒ � td|||j|j d� W 5 Q R X |j |j|j ¡dk�s@t‚|j |¡|j|j tdƒ k�sft‚| |¡t	tdƒd tdƒd dgtdƒ d tdƒd dgdddggƒk�s¸t‚| |¡tdƒk�sÐt‚d S )	NrR   r2   )Z
child_axisr   r”   r&   )Zparent_axisr8   )r   r.   r   r   r(   r¥   rG   rh   rs   r   r   r   r}   r¦   r   r   r)   r   )r<   r;   r   r    r!   r$   r,   r,   r-   Ú!test_deprecated_parent_child_axis´  s,    þ"&" ÿrì   c               	   C   s   t ƒ \} }}}tƒ �( td||| j| j |j|j d�}W 5 Q R X |j |j¡| j| j ks`t	‚|j
 |j¡|j|j ks~t	‚t ƒ \} }}}tƒ �( td||| j| j |j|j d�}W 5 Q R X |j |j¡| j| j ksÞt	‚|j
 |j¡|j|j ksüt	‚d S )Nr2   )Zparent_joint_posZchild_joint_pos)r.   r   r   r(   r)   re   rW   ra   rb   rG   rX   r   )r   r    r!   r$   rœ   Zsliderr,   r,   r-   Útest_deprecated_joint_posË  s    
ÿ
ÿrí   N)F)EZsympy.core.functionr   Zsympy.core.numbersr   Zsympy.core.singletonr   Z(sympy.functions.elementary.miscellaneousr   Z(sympy.functions.elementary.trigonometricr   r   Zsympyr   r	   r
   r   Zsympy.core.symbolr   Zsympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   Zsympy.physics.mechanics.jointr   Zsympy.physics.vectorr   r   r   Zsympy.testing.pytestr   r   Z_trJ   r.   r7   rP   rk   ru   rx   r~   r‡   r’   r�   rª   r­   r¯   r·   r¸   rº   r»   rÅ   rÊ   rÎ   rÔ   rÕ   rÙ   rå   rè   rë   rì   rí   r,   r,   r,   r-   Ú<module>   sP   4
/9I(/L8 6d17=>(