U
    L?hBâ  ã                   @   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,   úZ/var/www/html/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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: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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 )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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	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st‚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st‚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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_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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_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=>(