U
    —9%e+Ï  ã                   @   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 d d	lmZ d d
lmZm Z m!Z! d dl"m#Z#m$Z$ de_%ej&Z'dCd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„ Z0d d!„ Z1d"d#„ Z2d$d%„ Z3d&d'„ Z4d(d)„ Z5d*d+„ Z6d,d-„ Z7d.d/„ Z8d0d1„ Z9d2d3„ Z:d4d5„ Z;d6d7„ Z<d8d9„ Z=d:d;„ Z>d<d=„ Z?d>d?„ Z@d@dA„ ZAdBS )Dé    )Ú
expand_mul)Úpi)ÚS)Úsqrt)ÚcosÚsin)ÚMatrixÚ_simplify_matrixÚeyeÚzeros)Úsymbols)	ÚdynamicsymbolsÚBodyÚJointsMethodÚPinJointÚPrismaticJointÚCylindricalJointÚPlanarJointÚSphericalJointÚ	WeldJoint©ÚJoint)ÚVectorÚReferenceFrameÚPoint)ÚraisesÚwarns_deprecated_sympyTFc                 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/Darija-Ai-API/env/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*   ©r.   r-   r*   r+   Ú<lambda>$   ó    ztest_Joint.<locals>.<lambda>)r   r   Ú	TypeErrorr*   r*   r1   r+   Ú
test_Joint!   s    r5   c                     s0  t dƒ\} }}}t dƒ\}‰}}}}}	}
t dƒ\‰‰}}‰‰}}tƒ \}}‰‰ tdˆˆ ƒ‰ˆ | d¡t| gƒkstt‚ˆ |gd¡t|gƒkst‚ˆ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:4r0   é   r$   )Úoffseté   ÚuT)Znumber_singleÚqc                      s   ˆ   ˆˆgd¡S )Nr6   ©Ú_fill_coordinate_listr*   ©r0   Úq0Úq1r*   r+   r2   G   r3   z,test_coordinate_generation.<locals>.<lambda>c                      s   ˆ   ˆˆd gdd¡S )Nr$   r9   r;   r*   )r0   Úu0Úu1r*   r+   r2   H   r3   c                      s   ˆ   ˆˆgd¡S )Nr8   r;   r*   r=   r*   r+   r2   I   r3   c                      s   ˆ   ˆtdƒgd¡S )Nr?   r$   )r<   r   r*   )r0   r>   r*   r+   r2   K   r3   c                      s   ˆ   ˆˆ ˆgd¡S )Nr$   r;   r*   r=   r*   r+   r2   L   r3   c                      s   t dˆˆ ˆd d gƒS r/   ©r   r*   )r"   r   Úq1jr*   r+   r2   R   r3   c                      s   t dˆˆ ˆˆˆgdS )Nr0   )ÚspeedsrB   r*   )r"   r   r@   rA   r*   r+   r2   S   r3   )r   r,   r   r<   r   ÚAssertionErrorr   Ú
ValueErrorr4   ÚdiffÚt)r:   r9   ZqjZujZq0jZq2jZq3jZu0jZu1jZu2jZu3jÚq2Úq3Úu2Úu3Ú_r*   )r"   r0   r   r>   r?   rC   r@   rA   r+   Útest_coordinate_generation'   sF     ÿ  $ÿ"" ""rN   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_Jr0   r   z PinJoint: J  parent: P  child: CÚP1ÚC1r#   r$   ÚJ1©Úparent_pointÚchild_pointÚ
joint_axisÚparent_interframeúq, uTrU   rV   ©rU   rV   rX   Úchild_interframerW   )'r   r   r   r   ÚnamerE   r-   r.   Úcoordinatesr   rD   ÚkdesrG   rH   rW   r!   r&   rV   Úpos_fromÚ
masscenterr   rU   Ú_child_pointrX   r[   Ú__str__r   r%   r'   r   ÚzZ_joint_axisZ_parent_pointr,   Ú	locatenewÚvelÚexpressr   r   )r   r"   ÚlÚmr:   r9   ZPjrQ   rR   r(   rS   r   r   r)   rU   rV   r0   r*   r*   r+   Útest_pin_jointV   sx    
 þ  ÿ
  þ  ,, ÿri   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 u2zm lr   r   ÚBr"   r    r   ©r!   ÚmassÚRrS   )rD   r]   rV   rW   ÚJ2r   r6   )r   r   r   r   r   r&   r!   rc   Údcmr   r   r   rE   r	   Ú
ang_vel_inr^   rG   rH   r`   re   r'   )r?   rI   rA   rK   rh   rg   r   r   rj   r"   ZPartPZPartRrS   rn   r*   r*   r+   Útest_pin_joint_double_pendulum   sJ    
 ÿ
 ÿ ÿ ÿ(þ$    rq   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   rj   r$   é   Úrodrk   Úplate)rl   r!   r"   r    rS   )r]   rD   rV   rW   rn   )r]   rD   rU   rW   r   r6   )r   r   r   r   r   rc   r'   ro   r   r   r   rE   rp   r^   rG   rH   r`   r_   re   r&   )ZmAZmBZlAZlBÚhÚthetaÚphiÚomegaÚalphar   r   rj   ZlCrs   rt   r"   rS   rn   r*   r*   r+   Útest_pin_joint_chaos_pendulum·   sX     ÿ ÿþþ
&&ý&   ,ÿÿrz   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 )NrY   r(   r)   c                      s   t dˆˆ ˆdS ©Nr0   ©rX   ©r   r*   ©r"   r   r(   r*   r+   r2   ë   r3   z+test_pin_joint_interframe.<locals>.<lambda>c                      s   t dˆˆ ˆdS ©Nr0   ©r[   r}   r*   ©r"   r)   r   r*   r+   r2   ì   r3   c                      s   t dˆˆ ˆdS r{   r}   r*   r~   r*   r+   r2   ð   r3   c                      s   t dˆˆ ˆdS r   r}   r*   r   r*   r+   r2   ñ   r3   rr   r8   Zxyzr0   )rU   rV   rX   rW   g      à¿r$   é   éþÿÿÿr   )rU   rV   r[   rW   r6   rZ   )r   r,   r   r   rF   r%   rc   Zorient_body_fixedr   r   r&   r'   r	   ro   r   r   r   r   r   rE   rp   r`   r_   re   r   r   Úsimplify)r:   r9   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st‚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 )NrY   Tr0   ©rX   r[   rW   r   éÿÿÿÿc                      s(   t dˆˆ tˆƒˆj tˆƒˆj  dS ©Nr0   )rW   )r   r   r&   r   r'   r*   ©r"   r   r   r:   r*   r+   r2   Q  s   ÿz+test_pin_joint_joint_axis.<locals>.<lambda>c                      s   t dˆˆ ˆ jdS r‰   ©r   r&   r*   ©r"   r   r*   r+   r2   T  r3   c                      s   t dˆˆ ˆjˆ j dS r‰   ©r   r&   r'   r*   rŒ   r*   r+   r2   V  r3   c                      s   t dˆˆ ˆˆˆjˆ j dS ©Nr0   r‡   r   r*   r…   r*   r+   r2   W  s       
þc                      s   t dˆˆ ˆˆˆjˆj dS rŽ   r   r*   r…   r*   r+   r2   Z  s       
þc                      s   t dˆˆ ˆˆtdƒdS )Nr0   r   r‡   )r   r   r*   r…   r*   r+   r2   b  s       þc                      s   t dˆˆ ˆˆˆjˆj dS rŽ   )r   r'   r*   r…   r*   r+   r2   e  s       
þ)r   r,   r   r'   rW   rE   ro   r   r   r   rc   r&   r   rF   Ú	Exception)r9   r   Úpinr*   )r"   r)   r   r   r(   r:   r+   Útest_pin_joint_joint_axis2  sb     ÿ"ÿ ÿ$ÿ ÿ0ÿ ÿ"ÿ
ÿr‘   c            	      C   sJ  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st‚| |¡ |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	sBt‚|j |j¡|j|j k	sbt‚|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
sBt‚|j |¡ |j¡td k
sdt‚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ƒ ksþt‚| |¡ |¡ ¡ ||j ||j  ||j  tdƒ ks>t‚| |¡ ¡ t|d ƒks^t‚| |¡ |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sFt‚d S )NrP   r0   r€   r   rˆ   r$   ©r[   rV   r6   ©rX   rU   ©rU   rV   r[   r8   r‚   rr   é   é   úm n©rU   rV   r[   rX   éüÿÿÿrƒ   )r   r,   r   r&   Úangle_betweenrE   rf   ro   r   r   r   rp   Ú	magnituder   r`   r_   r„   re   r'   Úcrossrc   r   Úxreplacer   r	   r   r   Údot)	r:   r9   r   r   r   r"   Zanglerh   Únr*   r*   r+   Útest_pin_joint_arbitrary_axisj  sR   þ"þ  ÿ
"þ  
ÿ"*40ýÿ
 ( .ÿÿ
<ÿ
"ÿ(0&ÿ$ÿ*ÿû$ÿÿ 2 6 ÿÿ
$$ÿÿ
"þ&J(ÿÿ
<"ÿ<"ÿDÿû6$ÿÿ 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&   rc   rE   r'   )r   r   r   r"   Úfr*   r*   r+   Útest_create_aligned_frame_più  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 uTr0   )rX   r[   r   r6   r‡   c                      s   t dˆˆ ˆˆj dS r‰   )r   rc   r*   rŠ   r*   r+   r2     r3   z%test_pin_joint_axis.<locals>.<lambda>)r   r,   r   rW   r&   rE   r   r   r   rc   ro   r   rF   )r9   r   r(   r)   r0   ÚN_R_Ar*   rŠ   r+   Útest_pin_joint_axis  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 )Nr0   ©rU   Z	J_P_jointÚp)rU   rV   c                      s   t dˆˆ ˆj ˆ¡dS ©Nr0   r¦   )r   r&   Ú	to_matrixr*   )r"   r   r   r*   r+   r2   3  r3   z'test_locate_joint_pos.<locals>.<lambda>r:   c                      s   t dˆˆ ˆˆj dS r¨   r‹   r*   rŠ   r*   r+   r2   7  r3   c                      s   t dˆˆ ˆdS ©Nr0   )rV   r}   r*   ©r"   r   rV   r*   r+   r2   :  r3   c                      s   t dˆˆ ˆdS rª   r}   r*   r«   r*   r+   r2   =  r3   )r,   r   r'   rc   rU   r\   rE   r_   r`   rV   rd   r   r4   r   rF   r   )r   ÚjointrU   r*   )r"   r   r   rV   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_framer6   r0   r|   r   r:   c                      s   t dˆˆ ˆdS r{   r}   r*   )r"   r   rX   r*   r+   r2   O  r3   z)test_locate_joint_frame.<locals>.<lambda>c                      s   t dˆˆ ˆdS r   r}   r*   )r"   r   r[   r*   r+   r2   U  r3   )r,   r   r%   rc   r   rX   rE   rp   r[   r   r   rF   )r   r   r¬   r:   r*   )r"   r   r[   rX   r+   Útest_locate_joint_frame@  s(    ÿÿr®   c               	   C   s0  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‚| |¡dksRt‚| |¡dksft‚| ¡ dksxt‚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s.t‚|j |j¡| |jj ksPt‚|j |j¡| |j ||j  |	|j  ks„t‚|j |j¡||jj ks¤t‚|j |
¡tdƒks¾t‚| |¡dksÒt‚| |¡dksæt‚t ƒ \} } }}tdƒ}
|
 |j|jtd ¡ td||||jj |	|jj |jj|
d}|j|jjksPt‚|j |j¡|	|jj kspt‚|j |j¡||jj kst‚|j |j¡| |jj ks²t‚|j |j¡| | |jj |	|jj  ksät‚|j |j¡||jj kst‚| |¡dkst‚| |¡dks,t‚d S )	Núq_S, u_Sr   r   z&PrismaticJoint: S  parent: P  child: CrO   r#   r$   rT   )r,   r   r   r\   rE   r-   r.   r]   r   rD   r^   rG   rH   rW   r!   r&   rV   r_   r`   r   rU   re   rp   rb   r   r   r%   r'   r   rc   )rM   r   r"   r:   r9   r   r   r   rg   rh   r(   r*   r*   r+   Útest_sliding_jointX  sh     " 
 þ  "4 
 þ  "2 r°   c               	   C   st
  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 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sZt‚|j	 
|j	¡| d |j |j ks‚t‚|j	 
|j	¡ |¡| tdƒd  d |j tdƒd |j  tdƒd |j  ksÚt‚|j	 |¡||j ksöt‚|j	 |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j  ksPt‚| |¡dksdt‚| |¡dksxt‚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 ks4t‚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  k	st‚|j	 
|j	¡ |¡||d  tdƒ|  d  |j d
| d tdƒ|  d  |j  d| d tdƒ|  d  |j  k	sšt‚|j	 |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j  k	sît‚|j	 |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j  k
sHt‚| |¡dk
s\t‚| |¡dk
spt‚d S )Nr¯   r   r€   r   rˆ   r6   r’   r“   r”   r$   r8   r‚   r—   r˜   rƒ   )r   r,   r   r&   rš   rE   rf   ro   r   r`   r_   r„   re   rp   r'   r   rc   r	   r   r   )r:   r9   r   r   r   r"   rh   rŸ   r*   r*   r+   Ú!test_sliding_joint_arbitrary_axisŽ  s¬    .($0"$.0"$
ÿ&R(F0ÿ$,08:þ(<ÿ
Zþ402 "þNÿ
dÿ
TZ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st‚|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_Jr0   r   z(CylindricalJoint: J  parent: P  child: Cz
q0:2, u0:2úl, mTr$   )Úrotation_coordinateÚrotation_speedÚtranslation_speedrU   rV   rX   r[   rW   )!r,   r   r   r\   rE   r-   r.   r]   r   rD   r³   Ztranslation_coordinater´   rµ   r^   rG   rH   rW   r&   rV   r_   r`   r   rU   ra   re   rp   rX   r[   rb   r   r'   rc   )r   r   r   r"   Úq0_defÚq1_defÚu0_defÚu1_defÚCjr>   r?   r@   rA   rg   rh   r(   r)   r*   r*   r+   Útest_cylindrical_jointí  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st‚|
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_Jr0   r   z#PlanarJoint: J  parent: P  child: Cú
q0:3, u0:3r²   T)r³   Úplanar_coordinatesÚplanar_speedsrU   rV   rX   r[   )!r,   r   r   r\   rE   r-   r.   r]   r   rD   r³   r½   r´   r¾   r^   rG   rH   Úrotation_axisr&   Zplanar_vectorsr'   rc   rV   r_   r`   r   rU   re   rp   rX   r[   rb   r   )r   r   r   r"   r¶   r·   Zq2_defr¸   r¹   Zu2_defrº   Zr_P_Cr>   r?   rI   r@   rA   rK   rg   rh   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:3r0   r8   )rU   rV   rX   r[   r‚   r   r$   rr   g      Ð?r6   g      è?)r   r   r,   r   rc   r'   r   r¿   rf   Ú	normalizerE   rš   r   ro   r   r
   r   r   r   r	   r   )r>   r?   rI   r@   rA   rK   Úl1Úl2r   r   r   r"   r0   r¤   r*   r*   r+   Útest_planar_joint_advancedU  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\   rE   r-   r.   r]   r   rD   r^   rG   rH   rV   r_   r`   r   rU   re   rp   r   r   r&   r'   rc   rb   Ú	_rot_typeÚ
_rot_orderZ_amounts)r   r   r   r"   r>   r?   rI   r@   rA   rK   r   r*   r*   r+   Útest_spherical_jointp  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:3r6   r   )r]   rD   r   )r   r,   r   r]   r   rE   rD   r^   rp   r   r   r&   r'   rc   )r>   r?   rI   r@   rA   rK   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>   r?   r@   r*   r+   r2   «  r3   z-test_spherical_joint_coords.<locals>.<lambda>c                	      s&   t dˆˆ tˆˆˆˆgƒtˆˆˆgƒƒS rÌ   rÍ   r*   )	r"   r   r>   r?   rI   rJ   r@   rA   rK   r*   r+   r2   ­  s       ÿc                
      s&   t dˆˆ tˆˆˆgƒtˆˆˆˆgƒƒS rÌ   rÍ   r*   )	r"   r   r>   r?   rI   r@   rA   rK   Úu4r*   r+   r2   ¯  s       ÿ)	r   r,   r   r]   r   rE   rD   r   rF   )	Zq0sZq1sZq2sZu0sZu1sZu2sr   r   r   r*   )
r"   r   r>   r?   rI   rJ   r@   rA   rK   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‚| |¡ |¡|kstt‚|j |¡ |¡|kst‚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s"t‚t| |¡|
| ƒ ƒtd
ƒksFt‚| |¡ |¡|
|ƒksdt‚|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ˆƒ ˆ gˆtˆƒ tˆƒ ˆtˆƒ  gˆtˆƒ tˆƒ ˆtˆƒ  ggƒkspt‚|j |¡ |¡ttdƒ ˆtˆtd  ƒ tˆƒ ˆtˆtd  ƒ   gˆtˆƒ ˆ gˆtˆƒ ˆ ggƒksêt‚d S )Nr¼   r$   rr   Tr   ÚbodyrÇ   ©r]   rD   rU   rV   rX   r[   Úrot_typeÚ	rot_orderrÆ   r8   ©	r]   rD   rU   rV   rX   r[   rÒ   ZamountsrÓ   c                    s"   |   ˆ ˆˆˆ ˆˆˆˆˆˆˆˆi¡S r¡   ©r   ©Úexpr©r>   r?   rI   r@   rA   rK   r*   r+   r2   Ò  s   ÿz2test_spherical_joint_orient_body.<locals>.<lambda>ZBodYZyxz)r   r   r   r   r   r   r,   r   r&   r'   rc   rÈ   ÚupperrE   rÉ   r	   ro   r   rp   r©   r`   re   ©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$   rr   Tr   ÚspacerÇ   rÑ   ÚSPACEr8   c                    s"   |   ˆ ˆˆˆ ˆˆˆˆˆˆˆˆi¡S r¡   rÕ   rÖ   rØ   r*   r+   r2   	  s   ÿz3test_spherical_joint_orient_space.<locals>.<lambda>rÔ   ZSPaCeZzxyr6   )r   r   r   r   r   r   r,   r   r&   rc   r'   rÈ   rÙ   rE   rÉ   r	   ro   r   rp   r©   r`   re   rÚ   r*   rØ   r+   Ú!test_spherical_joint_orient_spaceï  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‚| |¡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‚| |¡dks*t‚| |¡dks>t‚| ¡ dksPt‚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s2t‚|j |j¡tdƒksNt‚|j |¡tdƒksht‚| |¡dks|t‚| |¡dkst‚|j|jks¢t‚t||ƒ d S )
NÚWr6   r   r8   z!WeldJoint: W  parent: P  child: CrO   r#   r$   )rU   rV   rX   )r,   r   r\   rE   r-   r.   r]   r   rD   r^   ÚTro   r
   rV   r_   r`   r   rU   re   r!   rp   rb   r   r   r%   r'   r   r&   rc   r   )	rM   r   r"   rß   r   r   rg   rh   r(   r*   r*   r+   Útest_weld_joint-  sF    
 ÿ  *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 )	NrP   r0   )Z
child_axisr   rˆ   r$   )Zparent_axisr6   )r   r,   r   r   r&   rš   rE   rf   ro   r   r   r   rp   r›   r   r   r'   r   )r:   r9   r   r   r   r"   r*   r*   r+   Ú!test_deprecated_parent_child_axisT  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 )Nr0   )Zparent_joint_posZchild_joint_pos)r,   r   r   r&   r'   rc   rU   r_   r`   rE   rV   r   )r   r   r   r"   r   Zsliderr*   r*   r+   Útest_deprecated_joint_posk  s    
ÿ
ÿrã   N)F)BZsympy.core.functionr   Zsympy.core.numbersr   Zsympy.core.singletonr   Z(sympy.functions.elementary.miscellaneousr   Z(sympy.functions.elementary.trigonometricr   r   Zsympy.core.backendr   r	   r
   r   Zsympy.core.symbolr   Zsympy.physics.mechanicsr   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simpZ_trH   r,   r5   rN   ri   rq   rz   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>   sN   ,
/9(/L8 6_17<>'