a
    {þd+Ï  ã                   @   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*   úq/var/www/html/stable-diffusion-webui/venv/lib/python3.9/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stJ ‚ˆ |gd¡t|gƒksJ ‚ˆj|gdddt|gƒks°J ‚ˆ d d¡t|gƒksÊJ ‚ˆ d gd¡t|gƒksæJ ‚ˆ ˆd d gd¡tˆˆ|gƒksJ ‚ˆ d d¡t|ˆ|gƒks,J ‚ˆ g d¡t|ˆ|gƒksLJ ‚ˆjg dddtˆ||gƒkspJ ‚ˆjˆd |gdddtˆ||gƒksšJ ‚ˆjd dddt||gƒks¼J ‚ˆ d dd	¡t|gƒksÚJ ‚ˆ g dd	¡t|||	gƒksüJ ‚ˆjd dd
dt|gƒksJ ‚ˆ g dd	dd
¡t|	gƒks>J ‚ˆ g dd¡t|ˆ|gƒks`J ‚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   r   Ú
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     ÿ  $ÿ"" ""rM   c                  C   s  t dƒ} t dƒ}tdƒ\}}tdƒ\}}td| |ƒ}|jdksBJ ‚|j| ksPJ ‚|j|ks^J ‚|jt|gƒksrJ ‚|j	t|gƒks†J ‚|j
t|| t¡ gƒks¤J ‚|j| jjks¶J ‚|j |j¡tdƒksÐJ ‚|j | j¡tdƒksêJ ‚|j |j¡tdƒksJ ‚|j | j¡tdƒks"J ‚|j| jks4J ‚|j|jksFJ ‚| ¡ dksXJ ‚t dƒ}t d	ƒ}td
ƒ}	|	 |j|jtd ¡ td||||jj ||jj |jj|	d}
|
j|jjksÄJ ‚|
j |j¡||jj ksäJ ‚|
j |j¡||jj ksJ ‚|
j |
j¡tdƒks J ‚|j |j¡| |jj ||jj  ksNJ ‚|
j|	ks^J ‚|
j|jkspJ ‚tdƒ\}}t dƒ\}}} }}	}| j !d|j|j ¡}|j !d|j|j ¡}td| ||||||	||jd
}|j|jksìJ ‚|j "|¡dksJ ‚|j|ksJ ‚|j|ks"J ‚|j | j¡|j|j ksBJ ‚|j |j¡|j|j ksbJ ‚|j | j¡|j|j |j |j ksŽJ ‚|j "|¡ #|¡|t$|ƒ |t%|ƒ  |j | t$|ƒ |t%|ƒ  |j  ksäJ ‚|j|	ksôJ ‚|j|ksJ ‚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, uTrT   rU   ©rT   rU   rW   Úchild_interframerV   )&r   r   r   r   Únamer-   r.   Úcoordinatesr   rD   ÚkdesrF   rG   rV   r!   r&   rU   Úpos_fromÚ
masscenterr   rT   Ú_child_pointrW   rZ   Ú__str__r   r%   r'   r   ÚzZ_joint_axisZ_parent_pointr,   Ú	locatenewÚvelÚexpressr   r   )r   r"   ÚlÚmr:   r9   ZPjrP   rQ   r(   rR   r   r   r)   rT   rU   r0   r*   r*   r+   Útest_pin_jointV   sp    þ  ÿ
þ  ,,ÿrh   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g d¢gƒksæJ ‚| |¡t	t
|ƒt|ƒ dgt|ƒt
|ƒdgg d¢gƒks&J ‚t| |¡ƒt	t
| | ƒt| | ƒ dgt| | ƒt
| | ƒdgg d¢gƒkszJ ‚| |¡||j ks”J ‚| |¡||j ks®J ‚| |¡||j ||j  ksÒJ ‚|jt	||  t¡ gƒksòJ ‚|jt	|| t¡ gƒksJ ‚|
j |¡|| |j ks2J ‚|j |¡|| |j ksRJ ‚|j |¡|| |j |||  |j  ks„J ‚d S )Nzq1 q2zu1 u2zm lr   r   ÚBr"   r    r   ©r!   ÚmassÚRrR   )rD   r\   rU   rV   ÚJ2r   ©r   r   r6   )r   r   r   r   r   r&   r!   rb   Údcmr   r   r   r	   Ú
ang_vel_inr]   rF   rG   r_   rd   r'   )r?   rH   rA   rJ   rg   rf   r   r   ri   r"   ZPartPZPartRrR   rm   r*   r*   r+   Útest_pin_joint_double_pendulum   sB    ÿÿÿÿ(þ$    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g d¢t
|ƒdt	|ƒggƒksüJ ‚|
 |¡tt	|ƒt
|ƒ dgt
|ƒt	|ƒdgg d¢gƒks<J ‚| |	¡tt	|ƒt	|ƒ t
|ƒt
|ƒ t	|ƒ gt
|ƒ t	|ƒ t	|ƒt
|ƒt
|ƒ gt
|ƒdt	|ƒggƒks°J ‚|
 |	¡||	j ksÊJ ‚|
 |¡| |
j ksæJ ‚|	 |¡| |	j ||
j  ksJ ‚|jt|| t¡ gƒks,J ‚|jt|| t¡ gƒksLJ ‚|j |j¡||
j ksjJ ‚|j |j¡| |
j ksŠJ ‚|j |	¡|d |d  | |
j ks¶J ‚|j |	¡|d |d  | |d |d  |  |
j ksöJ ‚d S )NzmA, mB, lA, lB, hztheta phi omega alphar   r   ri   r$   é   Úrodrj   Úplate)rk   r!   r"   r    rR   )r\   rD   rU   rV   rm   )r\   rD   rT   rV   r   ©r   r6   r   rn   )r   r   r   r   r   rb   r'   ro   r   r   r   rp   r]   rF   rG   r_   r^   rd   r&   )ZmAZmBZlAZlBÚhÚthetaÚphiÚomegaÚalphar   r   ri   ZlCrs   rt   r"   rR   rm   r*   r*   r+   Útest_pin_joint_chaos_pendulum·   sT    ÿÿþþ
&&ý&   ,ÿÿ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ÚJ ‚| |¡|ˆj
 ksôJ ‚ˆ j ˆj¡|j
|j ksJ ‚ˆ j |¡||j ks0J ‚ˆj ˆ¡tdƒksJJ ‚ˆ j ˆ¡||j ksfJ ‚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J ‚| |¡tdƒ| d |j
 tdƒ| d |j  ksBJ ‚ˆ j ˆj¡|j |j
 ksdJ ‚ˆ j |¡ ¡ tdƒ tdƒ | d	 |j tdƒ tdƒ | d	 |j  ksºJ ‚ˆ j ˆ¡tdƒksÔJ ‚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¦J ‚| |¡tdƒ| d ˆj
 tdƒ| d ˆj  ksâJ ‚ˆ j ˆj¡|j
|j |j ksJ ‚tdƒ tdƒ | d	 |j
 }ˆ j |¡ ¡ |ksBJ ‚ˆ j ˆ¡ ¡ |ks\J ‚ˆ j ˆ¡tdƒksvJ ‚d S )NrX   r(   r)   c                      s   t dˆˆ ˆdS ©Nr0   ©rW   ©r   r*   ©r"   r   r(   r*   r+   r2   ë   r3   z+test_pin_joint_interframe.<locals>.<lambda>c                      s   t dˆˆ ˆdS ©Nr0   ©rZ   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   )rT   rU   rW   rV   g      à¿r$   é   éþÿÿÿr   )rT   rU   rZ   rV   r6   rY   )r   r,   r   r   rE   r%   rb   Zorient_body_fixedr   r   r&   r'   r	   ro   r   r   r   r   r   rp   r_   r^   rd   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sJJ ‚ˆ |¡ttˆƒdtˆƒgg d¢tˆƒdtˆƒ ggƒksˆJ ‚tdƒ\‰}‰‰ ‰‰tdˆˆ ˆ| ˆˆˆjd}|jˆjksÆJ ‚ˆ |¡ttˆƒ dtˆƒgg d¢tˆƒdtˆƒggƒksJ ‚tƒ \‰}‰‰ tdˆˆ ˆ| ˆj	ˆ j	 ˆj	d}|jˆj	ksFJ ‚ˆ |¡ttˆƒ tˆƒ dgtˆƒ tˆƒdgg d¢gƒksŠJ ‚tƒ \‰}‰‰ tdˆˆ ˆ| ˆj	ˆ j	 ˆj
d}|jˆj
ksÊJ ‚ˆ |¡tg d¢dtˆƒtˆƒgdtˆƒtˆƒ ggƒks
J ‚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 )NrX   Tr0   ©rW   rZ   rV   r   ©r   éÿÿÿÿr   )r   r   rŠ   ©rŠ   r   r   c                      s(   t dˆˆ tˆƒˆj tˆƒˆj  dS ©Nr0   )rV   )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'   rV   ro   r   r   r   rb   r&   r   rE   Ú	Exception)r9   r   Úpinr*   )r"   r)   r   r   r(   r:   r+   Útest_pin_joint_joint_axis2  sZ    ÿ ÿÿ"ÿ
ÿ0ÿ
ÿ ÿ
ÿr”   c            	      C   sD  t dƒ\} }tƒ \}}}}td|||j d |j  |j¡dksFJ ‚|j |¡ |jks^J ‚| |¡tg d¢dt| ƒ t	| ƒ gdt	| ƒ t| ƒggƒks J ‚| 
|¡||j ks¸J ‚| 
|¡ ¡ t|d ƒksÖJ ‚|j |j¡dksìJ ‚|j |j¡ |¡ ¡ dksJ ‚|j |¡dks$J ‚tƒ \}}}}td|||j|jd |j |j¡dks`J ‚|j |¡|jksxJ ‚| |¡tdt| ƒ t	| ƒ gg d¢dt	| ƒ t| ƒggƒks¼J ‚| 
|¡||j ksÖJ ‚| 
|¡ |¡||j ksöJ ‚| 
|¡ ¡ t|d ƒksJ ‚| 
|¡ |j¡dks2J ‚|j |¡||j ksNJ ‚|j |j¡|j ksjJ ‚|j |j¡ |¡ ¡ t| ƒ|j t	| ƒ|j  ks¤J ‚|j |¡ |j¡td ksÆJ ‚tƒ \}}}}td|||j|jd	 |j |j¡dksJ ‚|j |¡|jksJ ‚| |¡tg d
¢t| ƒ dt	| ƒgt	| ƒdt| ƒggƒksZJ ‚| 
|¡||j kstJ ‚| 
|¡ |¡||j ks”J ‚| 
|¡ ¡ t|d ƒks´J ‚| 
|¡ |j¡}| |di¡dksÞJ ‚|j |¡dksôJ ‚|j |j¡|jksJ ‚tƒ \}}}}td|||j|j|j|j d t|j |j|j ¡ƒdks^J ‚|j|j  |¡ ¡ tdƒ|j ksˆJ ‚t| |¡ƒttdƒd tdƒ t| ƒ d tdƒ t	| ƒ d gtdƒd tdƒt| ƒ d tdƒt	| ƒ d gdt	| ƒ t| ƒggƒksJ ‚| 
|¡||j ks4J ‚| 
|¡ |¡ ¡ ||j ||j  tdƒ ksjJ ‚| 
|¡ ¡ t|d ƒksŠJ ‚| 
|¡ |j|j ¡}| |di¡dksºJ ‚|j |¡ ¡ ||j tdƒ ksâJ ‚|j |j¡|j|j ksJ ‚|j |j¡ |¡ ¡ dtdƒd  |j tdƒt| ƒ d |j  tdƒt	| ƒ d |j  ksjJ ‚|j |¡ |¡ ¡ tdƒ | t	| ƒ d |j tdƒ| t| ƒ d |j  ksÄJ ‚|j |¡ |j¡td ksæJ ‚tƒ \}}}}td|||j|j|j|j |j d t|j |j|j |j ¡ƒdksBJ ‚|j|j |j  |¡ ¡ tdƒ|j ksrJ ‚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sZJ ‚| 
|¡||j kstJ ‚| 
|¡ |¡ ¡ ||j ||j  ||j  tdƒ ks´J ‚| 
|¡ ¡ t|d ƒksÔJ ‚| 
|¡ |j|j |j ¡}| |di¡dk	s
J ‚|j |¡ ¡ ||j ||j  tdƒ k	s<J ‚|j |j¡|j|j k	s\J ‚|j |j¡ |¡ ¡ dtdƒd  |j tdƒt	| td  ƒ d |j  tdƒt| td  ƒ d |j  k	sÔJ ‚|j |¡ |¡ ¡ tdƒ| t| td  ƒ d |j tdƒ| t	| td  ƒ d |j  k
s<J ‚|j |¡ |j¡td k
s^J ‚tƒ \}}}}tdƒ\}}td||||j ||j |j|j |j |j|j |j d |j|j |j  |j|j |j ¡}t|ƒdk
sîJ ‚|j|j |j  |¡ ¡ dt| ƒ d tdƒd  |j tdƒd dt	| td  ƒ d  |j  dt| td  ƒ d tdƒd  |j  ks†J ‚t| |¡ƒttdƒd dt| ƒ d  dt	| td  ƒ d tdƒd  dt| td  ƒ d tdƒd  gdt| td  ƒ d tdƒd  dt| ƒ d tdƒd  dt	| td  ƒ d tdƒd  gdt	| td  ƒ d tdƒd  dt| td  ƒ d tdƒd  dt| ƒ d tdƒd  ggƒksÂJ ‚| 
|¡||j ||j  ||j  tdƒ ksøJ ‚| 
|¡ |¡ ¡ ||j ||j  ||j  tdƒ ks8J ‚| 
|¡ ¡ t|d ƒksXJ ‚| 
|¡ |j|j |j ¡}| |di¡dksŽJ ‚|j |¡ ¡ tdƒ| | d |j tdƒ| | d |j  ksØJ ‚|j |j¡||j ||j  ks J ‚|j |j¡ |¡ ¡ ||dt| ƒ d  d  |j |dt	| td  ƒ d  d |j  |dt| td  ƒ d  d |j  ksŒJ ‚|j |¡ |¡ ¡ d| | t	| ƒ d |j d| | t| td  ƒ d |j  d| | t	| td  ƒ d |j  ksJ ‚|j |¡ |j|j |j ¡ ¡ dks@J ‚d S )NrO   r0   r   r   r‹   r$   ©rZ   rU   ©r6   r   r   ©rW   rT   ru   r6   ©rT   rU   rZ   r8   rƒ   rr   é   é   úm n©rT   rU   rZ   rW   éüÿÿÿr„   )r   r,   r   r&   Úangle_betweenre   ro   r   r   r   rp   Ú	magnituder   r_   r^   r…   rd   r'   Zcrossrb   r   Úxreplacer   r	   r   r   Údot)	r:   r9   r   r   r   r"   Zanglerg   Ú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s2J ‚t ||j |j¡}|j|jksVJ ‚t ||j |j¡}|j|jkszJ ‚t ||j |j |j|j ¡}|j|jksªJ ‚t ||j |j |j|j ¡}|j|jksÚJ ‚t ||j |j |j|j ¡}|j|jksJ ‚t ||j |j |j |j|j |j ¡}|j|j |j|j ksVJ ‚d S ©N)r,   r   Z_create_aligned_interframer&   rb   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sFJ ‚tdtˆƒtˆƒgdtˆƒ tˆƒgg d¢gƒ}tdƒ\‰}‰‰ }}tdˆˆ ˆ| ||ˆjd ˆ 	|¡|ks¶J ‚tdƒ\‰}‰‰ }}tdˆˆ ˆ| |||j d ˆ 	|¡|ksøJ ‚tdƒ\‰}‰‰ }}t
t‡ ‡‡‡fdd	„ƒ d S )
Nzq uTr0   )rW   rZ   r   r–   rˆ   c                      s   t dˆˆ ˆˆj dS rŒ   )r   rb   r*   r   r*   r+   r2     r3   z%test_pin_joint_axis.<locals>.<lambda>)r   r,   r   rV   r&   r   r   r   rb   ro   r   rE   )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s6J ‚|j ˆj¡ˆjˆj ksTJ ‚|jˆ jksdJ ‚t ƒ \‰} ‰‰ ˆj 	dˆjˆj ¡}tdˆˆ |ˆ jd}|j|ksªJ ‚|jˆ jksºJ ‚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   ©rT   Z	J_P_jointÚp)rT   rU   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   )rU   r~   r*   ©r"   r   rU   r*   r+   r2   :  r3   c                      s   t dˆˆ ˆdS r­   r~   r*   r®   r*   r+   r2   =  r3   )r,   r   r'   rb   rT   r[   r^   r_   rU   rc   r   r4   r   rE   r   )r   ÚjointrT   r*   )r"   r   r   rU   r:   r+   Útest_locate_joint_pos"  s0    
ÿÿr°   c                     sê   t ƒ \} }‰‰ tdƒ‰ˆ | | jd¡ tdˆˆ ˆd}|jˆksDJ ‚|j | ¡dksXJ ‚|j|ksfJ ‚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   rW   r*   r+   r2   O  r3   z)test_locate_joint_frame.<locals>.<lambda>c                      s   t dˆˆ ˆdS r€   r~   r*   )r"   r   rZ   r*   r+   r2   U  r3   )r,   r   r%   rb   r   rW   rp   rZ   r   r   rE   )r   r   r¯   r:   r*   )r"   r   rZ   rW   r+   Útest_locate_joint_frame@  s(    ÿÿr±   c               	   C   s0  t ƒ \} } }}tdƒ\}}td||ƒ}|jdks4J ‚|j|ksBJ ‚|j|ksPJ ‚|jt|gƒksdJ ‚|jt|gƒksxJ ‚|j	t|| 
t¡ gƒks–J ‚|j|jjks¨J ‚|j |j¡tdƒksÂJ ‚|j |j¡tdƒksÜJ ‚|j |j¡| |jj ksüJ ‚|j |j¡| |jj ksJ ‚|j |j¡||jj ks>J ‚| |¡dksRJ ‚| |¡dksfJ ‚| ¡ dksxJ ‚t ƒ \}}}}tdƒ\}}	tdƒ}
|
 |j|jtd ¡ td||||jj |	|jj |jj|
d}|j|jjksîJ ‚|j |j¡|	|jj ksJ ‚|j |j¡||jj ks.J ‚|j |j¡| |jj ksPJ ‚|j |j¡| |j ||j  |	|j  ks„J ‚|j |j¡||jj ks¤J ‚|j |
¡tdƒks¾J ‚| |¡dksÒJ ‚| |¡dksæJ ‚t ƒ \} } }}tdƒ}
|
 |j|jtd ¡ td||||jj |	|jj |jj|
d}|j|jjksPJ ‚|j |j¡|	|jj kspJ ‚|j |j¡||jj ksJ ‚|j |j¡| |jj ks²J ‚|j |j¡| | |jj |	|jj  ksäJ ‚|j |j¡||jj ksJ ‚| |¡dksJ ‚| |¡dks,J ‚d S )	Núq_S, u_Sr   r   z&PrismaticJoint: S  parent: P  child: CrN   r#   r$   rS   )r,   r   r   r[   r-   r.   r\   r   rD   r]   rF   rG   rV   r!   r&   rU   r^   r_   r   rT   rd   rp   ra   r   r   r%   r'   r   rb   )rL   r   r"   r:   r9   r   r   r   rf   rg   r(   r*   r*   r+   Útest_sliding_jointX  sd     " þ  "4 þ  "2 r³   c               	   C   s`
  t dƒ\} }tƒ \}}}}td|||j d |j  |j¡dksFJ ‚|j |¡ |jks^J ‚| |¡tg d¢g d¢g d¢gƒks†J ‚|j 	|j¡| |j ks¢J ‚|j 	|j¡ |¡ 
¡ |  |j ksÊJ ‚|j |¡||j ksäJ ‚|j |¡ |¡| |j ksJ ‚| |¡dksJ ‚| |¡dks0J ‚tƒ \}}}}td|||j|jd |j |j¡dkslJ ‚|j |¡|jks„J ‚| |¡tg d¢g d	¢g d¢gƒks®J ‚|j |¡||j ksÊJ ‚|j |¡ |¡||j ksìJ ‚|j 	|j¡| |j |j ksJ ‚|j 	|j¡ |¡ 
¡ | |j |j ks>J ‚| |¡dksRJ ‚| |¡dksfJ ‚tƒ \}}}}td|||j|jd
 |j |j¡dks¢J ‚|j |¡|jksºJ ‚| |¡tg d¢g d¢g d¢gƒksäJ ‚|j |¡||j ks J ‚|j |¡ |¡||j ks"J ‚|j 	|j¡|j| |j  ksFJ ‚| |¡dksZJ ‚| |¡dksnJ ‚tƒ \}}}}td|||j|j|j|j d |j |j|j ¡dksºJ ‚|j|j  |¡tdƒ|j ksàJ ‚| |¡ttdƒd tdƒ d dgtdƒd tdƒd dgg d¢gƒks0J ‚|j 	|j¡| d |j |j ksXJ ‚|j 	|j¡ |¡| tdƒd  d |j tdƒd |j  ksžJ ‚|j |¡ |¡||j|j  tdƒ ksÎJ ‚|j |¡||j ksêJ ‚| |¡dksþJ ‚| |¡dksJ ‚tƒ \}}}}td|||j|j|j|j |j d |j |j|j |j ¡dksjJ ‚|j|j |j  |¡tdƒ|j ks–J ‚t| |¡ƒttdƒd tdƒ d tdƒd gtdƒd tdƒd tdƒd  tdƒd tdƒd  gtdƒ d tdƒd tdƒd  tdƒd tdƒd  ggƒksFJ ‚|j 	|j¡| d |j |j ksnJ ‚|j 	|j¡ |¡| tdƒd  d |j tdƒd |j  tdƒd |j  ksÆJ ‚|j |¡||j ksâJ ‚|j |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j  ks<J ‚| |¡dksPJ ‚| |¡dksdJ ‚tƒ \}}}}tdƒ\}}td||||j ||j |j|j |j |j|j |j d |j|j |j  |j|j |j ¡ 
¡ dksðJ ‚|j|j |j  |¡|j|j |j ks J ‚t| |¡ƒttdƒ d tdƒ d tdƒd gtdƒd tdƒd tdƒd gtdƒ d tdƒd tdƒd ggƒks¢J ‚|j 	|j¡|tdƒ|  d  |j tdƒ|  d |j  tdƒ|  d |j  ||j  k	sJ ‚|j 	|j¡ |¡||d  tdƒ|  d  |j d| d tdƒ|  d  |j  d| d tdƒ|  d  |j  k	s†J ‚|j |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j  k	sÚJ ‚|j |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j  k
s4J ‚| |¡dk
sHJ ‚| |¡dk
s\J ‚d S )Nr²   r   r   r   r‹   r‰   rn   r•   r–   r—   ru   r˜   r$   r6   r8   rƒ   r›   rœ   r„   )r   r,   r   r&   rž   re   ro   r   r_   r^   r…   rd   rp   r'   r   rb   r	   r   r   )r:   r9   r   r   r   r"   rg   r¢   r*   r*   r+   Ú!test_sliding_joint_arbitrary_axisŽ  s¬    (($*"$.*"$
ÿ&P(F0ÿ$,08:þ(<ÿ
Zþ402 "þNÿ
dÿ
TZr´   c                  C   s¸  t ƒ \} }}}tdƒ\}}}}td||ƒ}|jdks8J ‚|j|ksFJ ‚|j|ksTJ ‚|jt||gƒksjJ ‚|jt||gƒks€J ‚|j	|ksŽJ ‚|j
|ksœJ ‚|j|ksªJ ‚|j|ks¸J ‚|jt|| t¡ || t¡ gƒksâJ ‚|j| jksòJ ‚|j |j¡tdƒksJ ‚|j |j¡tdƒks*J ‚|j |j¡| | j ksJJ ‚|j |j¡|| j kshJ ‚|j | ¡|| j ks„J ‚| | ¡|| j ksžJ ‚|j| ks®J ‚|j|ks¾J ‚| ¡ dksÐJ ‚tdƒ\}	}
}}tdƒ\}}t dƒ\} }}}}}td|||	|||| j ||j ||d| j d	}|jt|	|gƒksHJ ‚|jt||gƒks`J ‚|j	|	kspJ ‚|j
|ks€J ‚|j|ksJ ‚|j|ks J ‚|jt||	 t¡ || t¡ gƒksÌJ ‚|jd| j ksâJ ‚|j |j¡||j ks J ‚|j |j¡|| j ksJ ‚|j |j¡| | j ks>J ‚|j |j¡|| j || j  ||j  kspJ ‚|j | ¡|| j || |j  ksšJ ‚| | ¡|| j ks´J ‚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_speedrT   rU   rW   rZ   rV   ) r,   r   r   r[   r-   r.   r\   r   rD   r¶   Ztranslation_coordinater·   r¸   r]   rF   rG   rV   r&   rU   r^   r_   r   rT   r`   rd   rp   rW   rZ   ra   r   r'   rb   )r   r   r   r"   Úq0_defÚq1_defÚu0_defÚu1_defÚCjr>   r?   r@   rA   rf   rg   r(   r)   r*   r*   r+   Útest_cylindrical_jointí  sb    * 


ý, ÿÿ
*r¾   c                  C   sh  t ƒ \} }}}tdƒ\}}}tdƒ\}}}	td||ƒ}
|
jdksDJ ‚|
j|ksRJ ‚|
j|ks`J ‚|
jt|||gƒksxJ ‚|
jt|||	gƒksJ ‚|
j	|ksžJ ‚|
j
t||gƒks´J ‚|
j|ksÂJ ‚|
jt||	gƒksØJ ‚|
jt|| t¡ || t¡ |	| t¡ gƒksJ ‚|
j| jks"J ‚|
j| j| jgks:J ‚|
j |j¡tdƒksVJ ‚|
j |j¡tdƒksrJ ‚|| j || j  }|
j |
j¡| ks J ‚|j |j¡|ks¸J ‚|
j | ¡|| j |	| j  ksÞJ ‚| | ¡|| j ksøJ ‚|
j| ksJ ‚|
j|ksJ ‚|
 ¡ dks*J ‚tdƒ\}}}}}}tdƒ\}}t dƒ\} }}}}}td|||||g||g|| j ||j ||d	
}
|
jt|||gƒks¨J ‚|
jt|||gƒksÂJ ‚|
j	|ksÒJ ‚|
j
t||gƒksêJ ‚|
j|ksúJ ‚|
jt||gƒksJ ‚|
jt|| t¡ || t¡ || t¡ gƒksJJ ‚|
j|jks\J ‚|
j|j|jgkstJ ‚|
j |j¡||j ks’J ‚|
j |j¡|| j ks°J ‚|
j |
j¡|| j || j  ksØJ ‚|j |j¡|| j || j  || j  ||j  ksJ ‚|j | ¡| | j || j  || |j  ksJJ ‚| | ¡|| j ksdJ ‚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_speedsrT   rU   rW   rZ   ) r,   r   r   r[   r-   r.   r\   r   rD   r¶   rÀ   r·   rÁ   r]   rF   rG   Úrotation_axisr&   Zplanar_vectorsr'   rb   rU   r^   r_   r   rT   rd   rp   rW   rZ   ra   r   )r   r   r   r"   r¹   rº   Zq2_defr»   r¼   Zu2_defr½   Zr_P_Cr>   r?   rH   r@   rA   rJ   rf   rg   r(   r)   r*   r*   r+   Útest_planar_joint  sr    ÿ&
ýÿ(ÿ&ÿ
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ªJ ‚|j |	¡|	j|	jtdƒ   	¡ ksÒJ ‚|j 
|j¡td ksìJ ‚| |	¡ | d|d|di¡tdƒksJ ‚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ÒJ ‚d S )Nr¿   zl1:3r0   r8   )rT   rU   rW   rZ   rƒ   r   r$   rr   g      Ð?r6   g      è?)r   r   r,   r   rb   r'   r   rÂ   re   Ú	normalizerž   r   ro   r    r
   r   r   r   r	   r   )r>   r?   rH   r@   rA   rJ   Ú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<J ‚|
j|ksJJ ‚|
j|ksXJ ‚|
jt|||gƒkspJ ‚|
jt|||	gƒksˆJ ‚|
j	t|| 
t¡ || 
t¡ |	| 
t¡ gƒks¾J ‚|
j |j¡tdƒksØJ ‚|
j |j¡tdƒksòJ ‚|
j |
j¡tdƒksJ ‚|j |j¡tdƒks*J ‚|j | ¡tdƒksDJ ‚| |¡| t|ƒ t|ƒ |t|ƒ  |j |t|ƒ t|ƒ |t|ƒ  |j  | t|ƒ |	 |j  ksºJ ‚| |¡|t|ƒ t|ƒ |t|ƒ  |j | t|ƒ t|ƒ |t|ƒ  |j  |t|ƒ |	 |j  ks.J ‚|
 ¡ dks@J ‚|
jdksPJ ‚|
jdks`J ‚|
jd u spJ ‚d S )Núq0:3_S, u0:3_Sr   r   z&SphericalJoint: S  parent: P  child: CÚBODYé{   )r,   r   r   r[   r-   r.   r\   r   rD   r]   rF   rG   rU   r^   r_   r   rT   rd   rp   r   r   r&   r'   rb   ra   Ú	_rot_typeÚ
_rot_orderZ_amounts)r   r   r   r"   r>   r?   rH   r@   rA   rJ   r   r*   r*   r+   Útest_spherical_jointp  s<    6."ÿÿÿ,$ÿÿÿrÍ   c                  C   s
  t dƒ\} }}t ddƒ\}}}tƒ \}}}}	td||	| ||g|||gd}
|
jt| ||gƒksbJ ‚|
jt|||gƒkszJ ‚|
jtg d¢ƒksJ ‚| |	¡| t|ƒ t|ƒ |t	|ƒ  |j
 |t	|ƒ t|ƒ |t|ƒ  |j  | t	|ƒ | |j  ksJ ‚d S )Nzq0:3r6   r   )r\   rD   )r   r   r   )r   r,   r   r\   r   rD   r]   rp   r   r   r&   r'   rb   )r>   r?   rH   r@   rA   rJ   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snJ ‚|jtˆˆˆgƒks†J ‚tƒ \}}‰‰ tdˆˆ tˆˆˆgƒtˆˆˆgƒƒ}|jtˆˆˆgƒksÐJ ‚|jtˆˆˆgƒksèJ ‚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?   rH   rI   r@   rA   rJ   r*   r+   r2   ­  s    ÿc                
      s&   t dˆˆ tˆˆˆgƒtˆˆˆˆgƒƒS rÏ   rÐ   r*   )	r"   r   r>   r?   rH   r@   rA   rJ   Úu4r*   r+   r2   ¯  s    ÿ)r   r,   r   r\   r   rD   r   rE   )	Zq0sZq1sZq2sZu0sZu1sZu2sr   r   r   r*   )
r"   r   r>   r?   rH   rI   r@   rA   rJ   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*J ‚|	jdks:J ‚t| |¡|  ƒtd
ƒksZJ ‚| |¡ |¡|kstJ ‚|j |¡ |¡|ksJ ‚tdƒ\}}}}}}td||ˆ ˆˆgˆˆˆg|j|j	 |j	 |j
 ||d	ˆˆ ˆfdd}	‡ ‡‡‡‡‡fdd„}
|	j ¡ d	ksJ ‚|	jdks"J ‚t| |¡|
| ƒ ƒtd
ƒksFJ ‚| |¡ |¡|
|ƒksdJ ‚|j |¡ |¡|
|ƒks„J ‚tdƒ\}}}}}}td||ˆ ˆˆgˆˆˆg|j|j	 |j	 |j
 ||ddd}	|	j ¡ d	ksèJ ‚|	jdksøJ ‚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J ‚| |¡ |¡tˆtˆƒ ˆ gˆtˆƒ tˆƒ ˆtˆƒ  gˆtˆƒ tˆƒ ˆtˆƒ  ggƒkspJ ‚|j |¡ |¡ttdƒ ˆtˆtd  ƒ tˆƒ ˆtˆtd  ƒ   gˆtˆƒ ˆ gˆtˆƒ ˆ ggƒksêJ ‚d S )Nr¿   r$   rr   Tr   ÚbodyrÊ   ©r\   rD   rT   rU   rW   rZ   Úrot_typeÚ	rot_orderrÉ   r8   ©	r\   rD   rT   rU   rW   rZ   rÕ   ZamountsrÖ   c                    s"   |   ˆ ˆˆˆ ˆˆˆˆˆˆˆˆi¡S r¤   ©r    ©Úexpr©r>   r?   rH   r@   rA   rJ   r*   r+   r2   Ò  s   ÿz2test_spherical_joint_orient_body.<locals>.<lambda>ZBodYZyxz)r   r   r   r   r   r   r,   r   r&   r'   rb   rË   ÚupperrÌ   r	   ro   r   rp   r¬   r_   rd   ©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&ÿüû
0 þ: þ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 J ‚|	jdks0J ‚t| |¡|  ƒtd
ƒksPJ ‚t| |¡ |¡ƒ|ksnJ ‚t|j |¡ |¡ƒ|ksŽJ ‚‡ ‡‡‡‡‡fdd„}
tdƒ\}}}}}}td||ˆ ˆˆgˆˆˆg|j|j	 |j |j
 ||d	ˆˆ ˆfdd}	|	j ¡ d	ksJ ‚|	jdks J ‚t| |¡|
| ƒ ƒtd
ƒksDJ ‚t| |¡ |¡ƒ|
|ƒksfJ ‚t|j |¡ |¡ƒ|
|ƒksŠJ ‚tdƒ\}}}}}}td||ˆ ˆˆgˆˆˆg|j|j	 |j |j
 ||ddd}	|	j ¡ d	ksîJ ‚|	jdksþJ ‚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J ‚t| |¡ |¡tˆ ˆtˆƒ  gˆ tˆ ƒ ˆtˆ ƒ tˆƒ  gˆtˆ ƒ ˆtˆ ƒ tˆƒ  ggƒ ƒtd
dƒks„J ‚t|j |¡ |¡tˆtˆ ƒ ˆtˆ ƒ tˆƒ  gˆtˆ ƒ ˆtˆ ƒ tˆƒ  gˆˆtˆ ƒ  ˆtˆƒ  ˆtˆ ƒ tˆƒ  ggƒ ƒtd
dƒks"J ‚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&   rb   r'   rË   rÜ   rÌ   r	   ro   r   rp   r¬   r_   rd   rÝ   r*   rÛ   r+   Ú!test_spherical_joint_orient_spaceï  s’    (4ÿ(8ÿ&û 0þ :þý  ý$"$ý8&ÿ*6(ÿü4 þþ
  ÿýürá   c            	      C   s°  t ƒ \} } }}td||ƒ}|jdks(J ‚|j|ks6J ‚|j|ksDJ ‚|jtƒ ksTJ ‚|jtƒ ksdJ ‚|jtddg ƒj	ks|J ‚| 
|¡tdƒks’J ‚|j |j¡tdƒks¬J ‚|j |j¡tdƒksÆJ ‚|j |j¡tdƒksàJ ‚|j |j¡tdƒksúJ ‚|j |j¡tdƒksJ ‚| |¡dks*J ‚| |¡dks>J ‚| ¡ dksPJ ‚t ƒ \}}}}tdƒ\}}tdƒ}| |j|jtd ¡ td||||jj ||jj |d	}|j |j¡||jj ksÌJ ‚|j |j¡||jj ksìJ ‚|j |j¡tdƒksJ ‚|j |j¡| |j ||j  ks2J ‚|j |j¡tdƒksNJ ‚|j |¡tdƒkshJ ‚| |¡dks|J ‚| |¡dksJ ‚|j|jks¢J ‚t||ƒ d S )
NÚWr6   r   r8   z!WeldJoint: W  parent: P  child: CrN   r#   r$   )rT   rU   rW   )r,   r   r[   r-   r.   r\   r   rD   r]   ÚTro   r
   rU   r^   r_   r   rT   rd   r!   rp   ra   r   r   r%   r'   r   r&   rb   r   )	rL   r   r"   râ   r   r   rf   rg   r(   r*   r*   r+   Útest_weld_joint-  sD    ÿ  *rä   c                  C   sú  t dƒ\} }tƒ \}}}}tƒ $ td|||j d W d   ƒ n1 sJ0    Y  |j  |j¡dkslJ ‚|j |¡ |jks„J ‚| |¡tg d¢dt	| ƒ t
| ƒ gdt
| ƒ t	| ƒggƒksÆJ ‚| |¡||j ksÞJ ‚| |¡ ¡ t|d ƒksüJ ‚tƒ \}}}}tƒ ( td|||j|j d W d   ƒ n1 s@0    Y  |j |j|j ¡dkshJ ‚|j |¡|j|j tdƒ ksŽJ ‚| |¡ttdƒd tdƒd dgtdƒ d tdƒd dgg d¢gƒksÞJ ‚| |¡tdƒksöJ ‚d S )	NrO   r0   )Z
child_axisr   r‹   r$   )Zparent_axisrn   )r   r,   r   r   r&   rž   re   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*    2þ8&" ÿrå   c                  C   s,  t ƒ \} }}}tƒ 2 td||| j| j |j|j d}W d   ƒ n1 sL0    Y  |j |j¡| j| j kstJ ‚|j	 |j¡|j|j ks’J ‚t ƒ \} }}}tƒ 2 t
d||| j| j |j|j d}W d   ƒ n1 sÞ0    Y  |j |j¡| j| j ksJ ‚|j	 |j¡|j|j ks(J ‚d S )Nr0   )Zparent_joint_posZchild_joint_pos)r,   r   r   r&   r'   rb   rT   r^   r_   rU   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_trG   r,   r5   rM   rh   rq   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>   sN   ,
/9(/L8 6_17<>'