o
    GZŽ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}| r:t 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,   úW/var/www/auris/lib/python3.10/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>#   s    ztest_Joint.<locals>.<lambda>)r   r   Ú	TypeErrorr,   r,   r3   r-   Ú
test_Joint    s   r6   c                     s   t dƒ\} }}}t dƒ\}‰}}}}}	}
t dƒ\‰‰}}‰‰}}tƒ \}}‰‰ tdˆˆ ƒ‰ˆ | d¡t| gƒks:J ‚ˆ |gd¡t|gƒksHJ ‚ˆj|gdddt|gƒksXJ ‚ˆ d d¡t|gƒkseJ ‚ˆ d gd¡t|gƒkssJ ‚ˆ ˆd d gd¡tˆˆ|gƒks…J ‚ˆ d d¡t|ˆ|gƒks”J ‚ˆ g d¡t|ˆ|gƒks£J ‚ˆjg dddtˆ||gƒks´J ‚ˆ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:4r2   é   r&   )Úoffseté   ÚuT)Znumber_singleÚqc                      ó   ˆ   ˆˆgd¡S )Nr7   ©Ú_fill_coordinate_listr,   ©r2   Úq0Úq1r,   r-   r4   F   ó    z,test_coordinate_generation.<locals>.<lambda>c                      s   ˆ   ˆˆd gdd¡S )Nr&   r:   r=   r,   )r2   Úu0Úu1r,   r-   r4   G   ó    c                      r<   )Nr9   r=   r,   r?   r,   r-   r4   H   rB   c                      s   ˆ   ˆtdƒgd¡S )NrA   r&   )r>   r   r,   )r2   r@   r,   r-   r4   J   rE   c                      s   ˆ   ˆˆ ˆgd¡S )Nr&   r=   r,   r?   r,   r-   r4   K   rE   c                      s   t dˆˆ ˆd d gƒS r1   ©r   r,   )r$   r!   Úq1jr,   r-   r4   Q   rE   c                      s   t dˆˆ ˆˆˆgdS )Nr2   )ÚspeedsrF   r,   )r$   r!   rC   rD   r,   r-   r4   R   ó    )
r   r.   r   r>   r   r   Ú
ValueErrorr5   ÚdiffÚt)r;   r:   ZqjZujZq0jZq2jZq3jZu0jZu1jZu2jZu3jÚq2Úq3Úu2Úu3Ú_r,   )r$   r2   r!   r@   rA   rG   rC   rD   r-   Útest_coordinate_generation&   sF    
ÿ"
ÿ   ""rR   c                  C   sø  t dƒ} t dƒ}tdƒ\}}tdƒ\}}td| |ƒ}|jdks!J ‚|j| ks(J ‚|j|ks/J ‚|jt|gƒks9J ‚|j	t|gƒksCJ ‚|j
t|| t¡ gƒksRJ ‚|j| jjks[J ‚|j |j¡tdƒkshJ ‚|j | j¡tdƒksuJ ‚|j |j¡tdƒks‚J ‚|j | j¡tdƒksJ ‚|j| jks—J ‚|j|jksŸJ ‚| ¡ dks§J ‚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sJ ‚|
j|	ks'J ‚|
j|jks0J ‚tdƒ\}}t dƒ\}}} }}	}| j !d|j|j ¡}|j !d|j|j ¡}td| ||||||	||jd
}|j|jksnJ ‚|j "|¡dksyJ ‚|j|ksJ ‚|j|ks‰J ‚|j | j¡|j|j ks™J ‚|j |j¡|j|j ks©J ‚|j | j¡|j|j |j |j ks¿J ‚|j "|¡ #|¡|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_Jr2   r   z PinJoint: J  parent: P  child: CÚP1ÚC1r%   r&   ÚJ1©Úparent_pointÚchild_pointÚ
joint_axisÚparent_interframeúq, uTrY   rZ   ©rY   rZ   r\   Úchild_interframer[   )&r   r   r   r   Únamer/   r0   Úcoordinatesr   rH   ÚkdesrK   rL   r[   r#   r(   rZ   Úpos_fromÚ
masscenterr   rY   Ú_child_pointr\   r_   Ú__str__r   r'   r)   r   ÚzZ_joint_axisZ_parent_pointr.   Ú	locatenewÚvelÚexpressr   r   )r!   r$   ÚlÚmr;   r:   ZPjrU   rV   r*   rW   r   r    r+   rY   rZ   r2   r,   r,   r-   Útest_pin_jointU   sp   þ
ÿþ  ,,ÿrm   c                  C   s&  t dƒ\} }tdƒ}tdƒ}td| d}tdƒ\}}td|||||||j d}|j|ks/J ‚|jjdks7J ‚|j	 
|j	¡| |j ksFJ ‚| |j¡tg d	¢d
t|ƒt|ƒgd
t|ƒ t|ƒggƒksfJ ‚|j	 |j¡| | |j kswJ ‚tdƒ}tdƒ}tdƒ}td| d}tdƒ\}}td||||||||j |jd	}|j|jjks©J ‚| |j¡tt|ƒt|ƒd
gt|ƒ t|ƒd
gg d¢gƒksÉJ ‚|j	 |j¡d
ksÔJ ‚|j	 |j¡|| |j ksäJ ‚tdƒ\}	}
}}}}dd„ |	|f|
|f||ffD ƒ}tdƒtdƒ}}td||||ƒ}|j |j¡tdƒksJ ‚|j	 
|j	¡||jj ks-J ‚|j	 |j¡d
ks9J ‚|j	 |j¡||jj ksIJ ‚tdƒtdƒ}}tdƒ}td|||	|
|||||j |jd
}|j |j¡tt|	ƒd
t|	ƒgg d¢t|	ƒ d
t|	ƒggƒksŠJ ‚|j	 
|j	¡||j |
|j  ksžJ ‚|j	 |j¡||j ks­J ‚|j	 |j¡ |¡| |j || |j  ksÇJ ‚tdƒtdƒ}}tdƒ}td|||	|
|g|||g|||j d	}|j}|j |j¡tg d	¢d
t|	ƒt|	ƒ gd
t|	ƒt|	ƒggƒksJ ‚|j	 
|j	¡| |j |
|j  ||j  ks'J ‚|j	 |j¡|| |j ||j  ||j  ksBJ ‚tdƒtdƒ}}tdƒtdƒ}}td||||||j ||j d}| |¡tdƒksqJ ‚|j	 
|j	¡||j ||j  ks…J ‚|j	 |j¡d
ks‘J ‚d S )Núm lr$   r!   )Úmassr]   r2   ©r_   rZ   Z	J_P_frame©r7   r   r   r   )r\   r_   rZ   r[   ©r   r   r7   z	q1:4 u1:4c                 S   s   i | ]
\}}|  t¡|“qS r,   )rK   rL   )Ú.0ZqiZuir,   r,   r-   Ú
<dictcomp>­   s    z/test_particle_compatibility.<locals>.<dictcomp>r9   ÚP_frame)r\   rY   r[   ©r   r7   r   ÚC_frame)r\   r_   rY   rZ   )r   r   r   r   r   r)   r_   r\   r`   rd   rc   Údcmr   r   r   ri   rg   r[   r(   r   r
   r   Úxreplacer   r   )rl   rk   rw   r!   r$   r;   r:   r2   ru   rA   rM   rN   rD   rO   rP   Z	qdot_to_ur,   r,   r-   Útest_particle_compatibilityŽ   sš   ÿþ"
þþ    ÿ(ÿ(
ÿ
ÿ(ÿ
ÿ 
ÿ

þ(rz   c                  C   s  t dƒ\} }tdƒ}tƒ  tdƒ}td| |d}W d   ƒ n1 s#w   Y  tdƒ\}}td||||||j d |j|ksBJ ‚|jjdksJJ ‚|j	 
|j	¡| |j ksYJ ‚|j |j¡tg d	¢d
t|ƒt|ƒgd
t|ƒ t|ƒggƒkszJ ‚|j	 |j¡| | |j ks‹J ‚d S )Nrn   r$   r!   ©ro   r#   r]   r2   ©rZ   ru   rq   r   )r   r   r   r   r   r   r)   r#   r`   rd   rc   rx   r   r   r   ri   rg   )rl   rk   rw   r!   r$   r;   r:   r,   r,   r-   Útest_body_compatibility×   s    þþ&r}   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ssJ ‚| |¡t	t
|ƒt|ƒ dgt|ƒt
|ƒdgg d¢gƒks’J ‚t| |¡ƒt	t
| | ƒt| | ƒ dgt| | ƒt
| | ƒdgg d¢gƒks»J ‚| |¡||j ksÇJ ‚| |¡||j ksÓJ ‚| |¡||j ||j  ksäJ ‚|jt	||  t¡ gƒksóJ ‚|jt	|| t¡ gƒksJ ‚|
j |¡|| |j ksJ ‚|j |¡|| |j ks#J ‚|j |¡|| |j |||  |j  ks<J ‚d S )Nzq1 q2zu1 u2rn   r   r    ÚBr$   r"   r!   ©r#   ro   ÚRrW   )rH   ra   rZ   r[   ÚJ2r   rr   )r   r   r   r   r   r(   r#   rg   rx   r   r   r   r	   Ú
ang_vel_inrb   rK   rL   rd   ri   r)   )rA   rM   rD   rO   rl   rk   r   r    r~   r$   ZPartPZPartRrW   r   r,   r,   r-   Útest_pin_joint_double_pendulumè   sB   ÿÿÿÿ(þ"   6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g d¢t
|ƒdt	|ƒggƒks~J ‚|
 |¡tt	|ƒt
|ƒ dgt
|ƒt	|ƒdgg d¢gƒksJ ‚| |	¡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s"J ‚|j |j¡||
j ks1J ‚|j |j¡| |
j ksAJ ‚|j |	¡|d |d  | |
j ksWJ ‚|j |	¡|d |d  | |d |d  |  |
j kswJ ‚d S )NzmA, mB, lA, lB, hztheta phi omega alphar   r    r~   r&   é   Úrodr   Úplater{   r$   r"   rW   )ra   rH   rZ   r[   r   )ra   rH   rY   r[   r   rv   rr   )r   r   r   r   r   rg   r)   rx   r   r   r   r‚   rb   rK   rL   rd   rc   ri   r(   )ZmAZmBZlAZlBÚhÚthetaÚphiÚomegaÚalphar   r    r~   ZlCr…   r†   r$   rW   r   r,   r,   r-   Útest_pin_joint_chaos_pendulum  sT   ÿÿþþ
&&ý&   ,ÿÿrŒ   c                     sv  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sJ ‚ˆj ˆ¡tdƒks#J ‚ˆ j ˆ¡||j ks1J ‚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sJ ‚ˆ j ˆj¡|j |j
 ks0J ‚ˆ j |¡ ¡ tdƒ tdƒ | d	 |j tdƒ tdƒ | d	 |j  ks[J ‚ˆ j ˆ¡tdƒkshJ ‚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sQJ ‚| |¡tdƒ| d ˆj
 tdƒ| d ˆj  ksoJ ‚ˆ j ˆj¡|j
|j |j ks‚J ‚tdƒ tdƒ | d	 |j
 }ˆ j |¡ ¡ |ksŸJ ‚ˆ j ˆ¡ ¡ |ks¬J ‚ˆ j ˆ¡tdƒks¹J ‚d S )Nr]   r*   r+   c                      ó   t dˆˆ ˆdS ©Nr2   ©r\   ©r   r,   ©r$   r!   r*   r,   r-   r4   D  rB   z+test_pin_joint_interframe.<locals>.<lambda>c                      ó   t dˆˆ ˆdS ©Nr2   ©r_   r   r,   ©r$   r+   r!   r,   r-   r4   E  rB   c                      r   rŽ   r   r,   r‘   r,   r-   r4   I  rB   c                      r’   r“   r   r,   r•   r,   r-   r4   J  rB   r„   r9   Zxyzr2   )rY   rZ   r\   r[   g      à¿r&   é   éþÿÿÿr   )rY   rZ   r_   r[   r7   r^   )r   r.   r   r   rJ   r'   rg   Zorient_body_fixedr   r   r(   r)   r	   rx   r   r   r   r   r   r‚   rd   rc   ri   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s%J ‚ˆ |¡ttˆƒdtˆƒgg d¢tˆƒdtˆƒ ggƒksDJ ‚tdƒ\‰}‰‰ ‰‰tdˆˆ ˆ| ˆˆˆjd}|jˆjkscJ ‚ˆ |¡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s¡J ‚ˆ |¡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 )Nr]   Tr2   ©r\   r_   r[   r   ©r   éÿÿÿÿr   )r   r   rœ   ©rœ   r   r   c                      s(   t dˆˆ tˆƒˆj tˆƒˆj  dS ©Nr2   )r[   )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   ­  s    c                      s   t dˆˆ ˆjˆ j dS rž   ©r   r(   r)   r,   r¡   r,   r-   r4   ¯  ó    c                      s   t dˆˆ ˆˆˆjˆ j dS ©Nr2   rš   r¢   r,   r˜   r,   r-   r4   °  ó    

þc                      s   t dˆˆ ˆˆˆjˆj dS r¤   r¢   r,   r˜   r,   r-   r4   ³  r¥   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   ¾  r¥   )r   r.   r   r)   r[   rx   r   r   r   rg   r(   r   rJ   Ú	Exception)r:   r    Úpinr,   )r$   r+   r   r!   r*   r;   r-   Útest_pin_joint_joint_axis‹  sZ   ÿ ÿÿ"ÿ
ÿ0ÿ
ÿ ÿ
ÿr¨   c            	      C   sB  t dƒ\} }tƒ \}}}}td|||j d |j  |j¡dks#J ‚|j |¡ |jks/J ‚| |¡tg d¢dt| ƒ t	| ƒ gdt	| ƒ t| ƒggƒksPJ ‚| 
|¡||j ks\J ‚| 
|¡ ¡ t|d ƒkskJ ‚|j |j¡dksvJ ‚|j |j¡ |¡ ¡ dks†J ‚|j |¡dksJ ‚tƒ \}}}}td|||j|jd |j |j¡dks­J ‚|j |¡|jks¸J ‚| |¡tdt| ƒ t	| ƒ gg d¢dt	| ƒ t| ƒggƒksÙJ ‚| 
|¡||j ksåJ ‚| 
|¡ |¡||j ksôJ ‚| 
|¡ ¡ t|d ƒksJ ‚| 
|¡ |j¡dksJ ‚|j |¡||j ks J ‚|j |j¡|j ks.J ‚|j |j¡ |¡ ¡ t| ƒ|j t	| ƒ|j  ksKJ ‚|j |¡ |j¡td ks\J ‚tƒ \}}}}td|||j|jd	 |j |j¡dkszJ ‚|j |¡|jks†J ‚| |¡tg d
¢t| ƒ dt	| ƒgt	| ƒdt| ƒggƒks¦J ‚| 
|¡||j ks³J ‚| 
|¡ |¡||j ksÃJ ‚| 
|¡ ¡ t|d ƒksÓJ ‚| 
|¡ |j¡}| |di¡dksèJ ‚|j |¡dksóJ ‚|j |j¡|jks J ‚tƒ \}}}}td|||j|j|j|j d t|j |j|j ¡ƒdks(J ‚|j|j  |¡ ¡ tdƒ|j ks=J ‚t| |¡ƒttdƒd tdƒ t| ƒ d tdƒ t	| ƒ d gtdƒd tdƒt| ƒ d tdƒt	| ƒ d gdt	| ƒ t| ƒggƒks†J ‚| 
|¡||j ks“J ‚| 
|¡ |¡ ¡ ||j ||j  tdƒ ks®J ‚| 
|¡ ¡ t|d ƒks¾J ‚| 
|¡ |j|j ¡}| |di¡dksÖJ ‚|j |¡ ¡ ||j tdƒ ksêJ ‚|j |j¡|j|j ksúJ ‚|j |j¡ |¡ ¡ dtdƒd  |j tdƒt| ƒ d |j  tdƒt	| ƒ d |j  ks.J ‚|j |¡ |¡ ¡ tdƒ | t	| ƒ d |j tdƒ| t| ƒ d |j  ks[J ‚|j |¡ |j¡td kslJ ‚tƒ \}}}}td|||j|j|j|j |j d t|j |j|j |j ¡ƒdksšJ ‚|j|j |j  |¡ ¡ tdƒ|j ks²J ‚t| |¡ƒttdƒd tdƒ t	| td  ƒ d tdƒt| td  ƒ d gtdƒd tdƒt| td  ƒ d tdƒt	| td  ƒ d gtdƒ d tdƒt| dt d  ƒ d tdƒt	| dt d  ƒ d ggƒks&J ‚| 
|¡||j ks3J ‚| 
|¡ |¡ ¡ ||j ||j  ||j  tdƒ ksSJ ‚| 
|¡ ¡ t|d ƒkscJ ‚| 
|¡ |j|j |j ¡}| |di¡ ¡ dks€J ‚|j |¡ ¡ ||j ||j  tdƒ ks™J ‚|j |j¡|j|j ks©J ‚|j |j¡ |¡ ¡ dtdƒd  |j tdƒt	| td  ƒ d |j  tdƒt| td  ƒ d |j  ksåJ ‚|j |¡ |¡ ¡ tdƒ| t| td  ƒ d |j tdƒ| t	| td  ƒ d |j  ksJ ‚|j |¡ |j¡td ks*J ‚tƒ \}}}}tdƒ\}}td||||j ||j |j|j |j |j|j |j d |j|j |j  |j|j |j ¡}t|ƒdksrJ ‚|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ƒ   ¡ syJ ‚| 
|¡ |¡ ¡ ||j ||j  ||j  tdƒ ks™J ‚| 
|¡ ¡ t|d ƒks©J ‚| 
|¡ |j|j |j ¡}| |di¡ ¡ dksÆJ ‚|j |¡ ¡ tdƒ| | d |j tdƒ| | d |j  ksëJ ‚|j |j¡||j ||j  ksÿJ ‚|j |j¡ |¡ ¡ ||dt| ƒ d  d  |j |dt	| td  ƒ d  d |j  |dt| td  ƒ d  d |j  ksEJ ‚|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 )NrT   r2   r”   r   r   r&   rp   rq   ©r\   rY   rv   r7   ©rY   rZ   r_   r9   r–   r„   é   é   úm n©rY   rZ   r_   r\   éüÿÿÿr—   )r   r.   r   r(   Úangle_betweenrj   rx   r   r   r   r‚   Ú	magnituder   rd   rc   r	   ri   r)   Úcrossrg   r   ry   r   r   r   Údot)	r;   r:   r   r    r!   r$   Zanglerl   Únr,   r,   r-   Útest_pin_joint_arbitrary_axisÃ  sR  þ þ 
ÿ"þ  
ÿ"*40ý
ÿ ( .ÿ
ÿ<
ÿ"ÿ(0&ÿ$ÿ*ÿû$ÿÿ 2 6 ÿ
ÿ$$ÿ
ÿ"þ&J(ÿ
ÿ<"ÿ<"ÿDÿû:$ÿÿ 2
ÿ(4ÿÿÿ$ÿ
ÿB$ÿ
ÿ2rµ   c                  C   sV  t ƒ \} }}}t ||j |j¡}|j|jksJ ‚t ||j |j¡}|j|jks+J ‚t ||j |j¡}|j|jks=J ‚t ||j |j |j|j ¡}|j|jksUJ ‚t ||j |j |j|j ¡}|j|jksmJ ‚t ||j |j |j|j ¡}|j|jks…J ‚t ||j |j |j |j|j |j ¡}|j|j |j|j ks©J ‚d S ©N)r.   r   Z_create_aligned_interframer(   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s#J ‚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 uTr2   )r\   r_   r   rq   rš   c                      ó   t dˆˆ ˆˆj dS rž   )r   rg   r,   rŸ   r,   r-   r4   x  rI   z%test_pin_joint_axis.<locals>.<lambda>)r   r.   r   r[   r(   r   r   r   rg   rx   r   rJ   )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sJ ‚|j ˆj¡ˆjˆj ks*J ‚|jˆ jks2J ‚t ƒ \‰} ‰‰ ˆj 	dˆjˆj ¡}tdˆˆ |ˆ jd}|j|ksUJ ‚|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 )Nr2   ©rY   Z	J_P_jointÚp)rY   rZ   c                      s   t dˆˆ ˆj ˆ¡dS ©Nr2   r¼   )r   r(   Ú	to_matrixr,   )r$   r   r!   r,   r-   r4   Œ  r£   z'test_locate_joint_pos.<locals>.<lambda>r;   c                      r¹   r¾   r    r,   rŸ   r,   r-   r4     rI   c                      r   ©Nr2   r|   r   r,   ©r$   r!   rZ   r,   r-   r4   “  rB   c                      r   rÀ   r   r,   rÁ   r,   r-   r4   –  rB   )r.   r   r)   rg   rY   r`   rc   rd   rZ   rh   r   r5   r   rJ   r   )r    ÚjointrY   r,   )r$   r   r!   rZ   r;   r-   Útest_locate_joint_pos{  s0   
ÿÿrÃ   c                     sê   t ƒ \} }‰‰ tdƒ‰ˆ | | jd¡ tdˆˆ ˆd}|jˆks"J ‚|j | ¡dks,J ‚|j|ks3J ‚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_framer7   r2   r   r   r;   c                      r   rŽ   r   r,   )r$   r!   r\   r,   r-   r4   ¨  rB   z)test_locate_joint_frame.<locals>.<lambda>c                      r   r“   r   r,   )r$   r!   r_   r,   r-   r4   ®  rB   )r.   r   r'   rg   r   r\   r‚   r_   r   r   rJ   )r   r    rÂ   r;   r,   )r$   r!   r_   r\   r-   Útest_locate_joint_frame™  s(   ÿÿrÄ   c               	   C   s<  t ƒ \} } }}tdƒ\}}td||ƒ}|jdksJ ‚|j|ks!J ‚|j|ks(J ‚|jt|gƒks2J ‚|jt|gƒks<J ‚|j	t|| 
t¡ gƒksKJ ‚|j|jjksTJ ‚|j |j¡tdƒksaJ ‚|j |j¡tdƒksnJ ‚|j |j¡| |jj ks~J ‚|j |j¡| |jj ksŽJ ‚|j |j¡||jj ksJ ‚|j |j¡dks¨J ‚|j |j¡dks³J ‚| ¡ dks»J ‚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s&J ‚|j |j¡| |j ||j  |	|j  ks@J ‚|j |j¡||jj ksPJ ‚|j |
¡tdƒks]J ‚|j |j¡dksiJ ‚|j |j¡dksuJ ‚t ƒ \} } }}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sÛJ ‚|j |j¡| | |jj |	|jj  ksôJ ‚|j |j¡||jj ksJ ‚|j |j¡dksJ ‚|j |j¡dksJ ‚d S )	Núq_S, u_Sr   r   z&PrismaticJoint: S  parent: P  child: CrS   r%   r&   rX   )r.   r   r   r`   r/   r0   ra   r   rH   rb   rK   rL   r[   r#   r(   rZ   rc   rd   r   rY   ri   r‚   rf   r   r   r'   r)   r   rg   )rQ   r!   r$   r;   r:   r   r   r    rk   rl   r*   r,   r,   r-   Útest_prismatic_joint±  sd     þ  "4 þ  "2 rÆ   c               	   C   sš
  t dƒ\} }tƒ \}}}}td|||j d |j  |j¡dks#J ‚|j |¡ |jks/J ‚| |¡tg d¢g d¢g d¢gƒksCJ ‚|j 	|j¡| |j ksQJ ‚|j 	|j¡ |¡ 
¡ |  |j kseJ ‚|j |¡||j ksrJ ‚|j |¡ |¡| |j ksƒJ ‚| |¡dksŒJ ‚| |¡dks•J ‚tƒ \}}}}td|||j|jd |j |j¡dks²J ‚|j |¡|jks½J ‚| |¡tg d¢g d	¢g d¢gƒksÑJ ‚|j |¡||j ksÞJ ‚|j |¡ |¡||j ksîJ ‚|j 	|j¡| |j |j ksÿJ ‚|j 	|j¡ |¡ 
¡ | |j |j ksJ ‚| |¡dks J ‚| |¡dks*J ‚tƒ \}}}}td|||j|jd
 |j |j¡dksHJ ‚|j |¡|jksTJ ‚| |¡tg d¢g d¢g d¢gƒksiJ ‚|j |¡||j kswJ ‚|j |¡ |¡||j ksˆJ ‚|j 	|j¡|j| |j  ksšJ ‚| |¡dks¤J ‚| |¡dks®J ‚tƒ \}}}}td|||j|j|j|j d |j |j|j ¡dksÔJ ‚|j|j  |¡tdƒ|j ksçJ ‚| |¡ttdƒd tdƒ d dgtdƒd tdƒd dgg d¢gƒksJ ‚|j 	|j¡| d |j |j ks#J ‚|j 	|j¡ |¡| tdƒd  d |j tdƒd |j  ksFJ ‚|j |¡ |¡||j|j  tdƒ ks^J ‚|j |¡||j kslJ ‚| |¡dksvJ ‚| |¡dks€J ‚tƒ \}}}}td|||j|j|j|j |j d |j |j|j |j ¡ 
¡ dks®J ‚|j|j |j  |¡tdƒ|j   
¡ dksÈJ ‚t
| |¡ƒttdƒd tdƒ d tdƒd gtdƒd tdƒd tdƒd  tdƒd tdƒd  gtdƒ d tdƒd tdƒd  tdƒd tdƒd  ggƒks J ‚|j 	|j¡| d |j |j ks4J ‚|j 	|j¡ |¡| tdƒd  d |j tdƒd |j  tdƒd |j    
¡ dksdJ ‚|j |¡||j ksrJ ‚|j |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j    
¡ s¡J ‚| |¡dks«J ‚| |¡dksµJ ‚tƒ \}}}}tdƒ\}}td||||j ||j |j|j |j |j|j |j d |j|j |j  |j|j |j ¡ 
¡ dksûJ ‚|j|j |j  |¡|j|j |j   
¡ d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sXJ ‚|j 	|j¡|tdƒ|  d  |j tdƒ|  d |j  tdƒ|  d |j  ||j    |¡ 
¡ dks‘J ‚|j 	|j¡ |¡||d  tdƒ|  d  |j d| d tdƒ|  d  |j  d| d tdƒ|  d  |j    
¡ dksÕJ ‚|j |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j    
¡ dksJ ‚|j |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j    
¡ dks7J ‚| |¡dksAJ ‚| |¡dksKJ ‚d S )NrÅ   r   r”   r   r   r›   rr   rp   rq   r©   rv   rª   r&   r7   r9   r–   r­   r®   r—   )r   r.   r   r(   r°   rj   rx   r   rd   rc   r	   ri   r‚   r)   r   rg   r   r   )r;   r:   r   r    r!   r$   rl   r´   r,   r,   r-   Ú#test_prismatic_joint_arbitrary_axisç  sÔ   (("( ".*"$
ÿ&P(F0ÿ(408:þ(<ÿ
ÿ@ÿÿþ482 "þNÿ
þB ÿÿ
þ@ÿ
ÿ@ÿ
ÿrÇ   c                  C   s¦  t ƒ \} }}}tdƒ\}}}}td||ƒ}|jdksJ ‚|j|ks#J ‚|j|ks*J ‚|jt||gƒks5J ‚|jt||gƒks@J ‚|j	|ksGJ ‚|j
|ksNJ ‚|j|ksUJ ‚|j|ks\J ‚|jt|| t¡ || t¡ gƒksqJ ‚|j| jksyJ ‚|j |j¡tdƒks†J ‚|j |j¡tdƒks“J ‚|j |j¡| | j ks¢J ‚|j |j¡|| j ks°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|||	|||| j ||j ||d| j d	}|jt|	|gƒksJ ‚|jt||gƒks'J ‚|j	|	ks/J ‚|j
|ks7J ‚|j|ks?J ‚|j|ksGJ ‚|jt||	 t¡ || t¡ gƒks]J ‚|jd| j kshJ ‚|j |j¡||j kswJ ‚|j |j¡|| j ks†J ‚|j |j¡| | j ks–J ‚|j |j¡|| j || j  ||j  ks¯J ‚|j | ¡|| j || |j  ksÄJ ‚| | ¡|| j ksÑJ ‚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_speedrY   rZ   r\   r_   r[   ) r.   r   r   r`   r/   r0   ra   r   rH   rÉ   Ztranslation_coordinaterÊ   rË   rb   rK   rL   r[   r(   rZ   rc   rd   r   rY   re   ri   r‚   r\   r_   rf   r   r)   rg   )r   r    r!   r$   Úq0_defÚq1_defÚu0_defÚu1_defÚCjr@   rA   rC   rD   rk   rl   r*   r+   r,   r,   r-   Útest_cylindrical_jointK  sb   *


ý, ÿ
ÿ*rÑ   c                  C   sT  t ƒ \} }}}tdƒ\}}}tdƒ\}}}	td||ƒ}
|
jdks"J ‚|
j|ks)J ‚|
j|ks0J ‚|
jt|||gƒks<J ‚|
jt|||	gƒksHJ ‚|
j	|ksOJ ‚|
j
t||gƒksZJ ‚|
j|ksaJ ‚|
jt||	gƒkslJ ‚|
jt|| t¡ || t¡ |	| t¡ gƒks‡J ‚|
j| jksJ ‚|
j| j| jg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¡|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sJJ ‚|
jt|||gƒksWJ ‚|
j	|ks_J ‚|
j
t||gƒkskJ ‚|
j|kssJ ‚|
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¡||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sJ ‚| | ¡|| j ks(J ‚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_speedsrY   rZ   r\   r_   ) r.   r   r   r`   r/   r0   ra   r   rH   rÉ   rÓ   rÊ   rÔ   rb   rK   rL   Úrotation_axisr(   Zplanar_vectorsr)   rg   rZ   rc   rd   r   rY   ri   r‚   r\   r_   rf   r   )r   r    r!   r$   rÌ   rÍ   Zq2_defrÎ   rÏ   Zu2_defrÐ   Zr_P_Cr@   rA   rM   rC   rD   rO   rk   rl   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sUJ ‚|j |	¡|	j|	jtdƒ   	¡ ksiJ ‚|j 
|j¡td ksvJ ‚| |	¡ | 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:3r2   r9   )rY   rZ   r\   r_   r–   r   r&   r„   g      Ð?r7   g      è?)r   r   r.   r   rg   r)   r   rÕ   rj   Ú	normalizer°   r   rx   ry   r
   r   r   r   r	   r   )r@   rA   rM   rC   rD   rO   Úl1Úl2r   r    r!   r$   r2   rº   r,   r,   r-   Útest_planar_joint_advanced³  s*   
ü(((&$ÿ2ü"rÚ   c                  C   sl  t ƒ \} }}}tdƒ\}}}}}}	td||ƒ}
|
jdksJ ‚|
j|ks%J ‚|
j|ks,J ‚|
jt|||gƒks8J ‚|
jt|||	gƒksDJ ‚|
j	t|| 
t¡ || 
t¡ |	| 
t¡ gƒks_J ‚|
j |j¡tdƒkslJ ‚|
j |j¡tdƒksyJ ‚|
j |
j¡tdƒks†J ‚|j |j¡tdƒks“J ‚|j | ¡tdƒksŸJ ‚|  |¡| 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s$J ‚|
jdks,J ‚|
jd u s4J ‚d S )Núq0:3_S, u0:3_Sr   r   z&SphericalJoint: S  parent: P  child: CÚBODYé{   )r.   r   r   r`   r/   r0   ra   r   rH   rb   rK   rL   rZ   rc   rd   r   rY   ri   r‚   r   r   r(   r)   rg   rf   Ú	_rot_typeÚ
_rot_orderZ_amounts)r   r    r!   r$   r@   rA   rM   rC   rD   rO   r   r,   r,   r-   Útest_spherical_jointÎ  s<   6."ÿÿ
ÿ,$ÿÿÿrà   c                  C   s  t dƒ\} }}t ddƒ\}}}tƒ \}}}}	td||	| ||g|||gd}
|
jt| ||gƒks1J ‚|
jt|||gƒks=J ‚|
jtg d¢ƒksHJ ‚| |¡| t|ƒ t|ƒ |t	|ƒ  |j
 |t	|ƒ t|ƒ |t|ƒ  |j  | t	|ƒ | |j  ks‚J ‚d S )Nzq0:3r7   r   )ra   rH   )r   r   r   )r   r.   r   ra   r   rH   rb   r‚   r   r   r(   r)   rg   )r@   rA   rM   rC   rD   rO   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s7J ‚|jtˆˆˆgƒksCJ ‚tƒ \}}‰‰ tdˆˆ tˆˆˆgƒtˆˆˆgƒƒ}|jtˆˆˆgƒkshJ ‚|jtˆˆˆgƒkstJ ‚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   rC   r,   r-   r4   	  s    z-test_spherical_joint_coords.<locals>.<lambda>c                	      s&   t dˆˆ tˆˆˆˆgƒtˆˆˆgƒƒS râ   rã   r,   )	r$   r!   r@   rA   rM   rN   rC   rD   rO   r,   r-   r4     ó     ÿc                
      s&   t dˆˆ tˆˆˆgƒtˆˆˆˆgƒƒS râ   rã   r,   )	r$   r!   r@   rA   rM   rC   rD   rO   Úu4r,   r-   r4     rä   )r   r.   r   ra   r   rH   r   rJ   )	Zq0sZq1sZq2sZu0sZu1sZu2sr   r    r   r,   )
r$   r!   r@   rA   rM   rN   rC   rD   rO   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s-J ‚t| |¡ |¡| ƒtd
dƒksAJ ‚t|j |¡ |¡ƒ|ksQJ ‚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s¬J ‚t| |¡ |¡|
|ƒ ƒtd
dƒksÂJ ‚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ƒ ƒtd
ƒk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dƒ ˆtˆtd  ƒ tˆƒ ˆtˆtd  ƒ   gˆtˆƒ ˆ gˆtˆƒ ˆ ggƒksJ ‚d S )NrÒ   r&   r„   Tr   ÚbodyrÝ   ©ra   rH   rY   rZ   r\   r_   Úrot_typeÚ	rot_orderrÜ   r9   r7   ©	ra   rH   rY   rZ   r\   r_   ré   Zamountsrê   c                    ó"   |   ˆ ˆˆˆ ˆˆˆˆˆˆˆˆi¡S r¶   ©ry   ©Úexpr©r@   rA   rM   rC   rD   rO   r,   r-   r4   0  ó    ÿz2test_spherical_joint_orient_body.<locals>.<lambda>ZBodYZyxz)r   r   r   r   r   r   r.   r   r(   r)   rg   rÞ   Úupperrß   r	   rx   r   r‚   r¿   rd   ri   ©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sJ ‚t| |¡|  ƒtd
ƒks(J ‚t| |¡ |¡ƒ|ks7J ‚t|j |¡ |¡ƒ|ksGJ ‚‡ ‡‡‡‡‡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sJ ‚t| |¡|
| ƒ ƒtd
ƒks¢J ‚t| |¡ |¡ƒ|
|ƒks³J ‚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&   r„   Tr   ÚspacerÝ   rè   ÚSPACEr9   c                    rì   r¶   rí   rî   rð   r,   r-   r4   h  rñ   z3test_spherical_joint_orient_space.<locals>.<lambda>rë   ZSPaCeZzxyr7   )r   r   r   r   r   r   r.   r   r(   rg   r)   rÞ   rò   rß   r	   rx   r   r‚   r¿   rd   ri   ró   r,   rð   r-   Ú!test_spherical_joint_orient_spaceN  s’   (4ÿ(8ÿ&û 0þ :þý  ý$"$ý8&ÿ*6(ÿü4 þ
þ  ÿýür÷   c            	      C   sâ  t ƒ \} } }}td||ƒ}|jdksJ ‚|j|ksJ ‚|j|ks"J ‚|jtƒ ks*J ‚|jtƒ ks2J ‚|jtddg ƒj	ks>J ‚|j
 |j
¡tdƒksKJ ‚|j |j¡tdƒksXJ ‚|j |j¡tdƒkseJ ‚|j |j¡tdƒksrJ ‚|j |j¡tdƒksJ ‚|j |j
¡tdƒksŒJ ‚|j
 |j
¡dks—J ‚|j
 |j
¡dks¢J ‚| ¡ dksªJ ‚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sJ ‚|j |j
¡tdƒks'J ‚|j |¡tdƒks4J ‚|j
 |j
¡dks@J ‚|j
 |j
¡dksLJ ‚|j|jksUJ ‚tƒ  t||ƒ W d   ƒ d S 1 sjw   Y  d S )
NÚWr7   r   r9   z!WeldJoint: W  parent: P  child: CrS   r%   r&   )rY   rZ   r\   )r.   r   r`   r/   r0   ra   r   rH   rb   ÚTr#   rx   r
   rZ   rc   rd   r   rY   ri   r‚   rf   r   r   r'   r)   r   r(   rg   r   r   )	rQ   r!   r$   rø   r   r    rk   rl   r*   r,   r,   r-   Útest_weld_jointŒ  sH   ÿ*$ÿrú   c                  C   sð  t dƒ\} }tƒ \}}}}tƒ  td|||j d W d   ƒ n1 s%w   Y  |j  |j¡dks6J ‚|j |¡ |jksBJ ‚| |¡tg d¢dt	| ƒ t
| ƒ gdt
| ƒ t	| ƒggƒkscJ ‚| |¡||j ksoJ ‚| |¡ ¡ t|d ƒks~J ‚tƒ \}}}}tƒ  td|||j|j d W d   ƒ n1 sŸw   Y  |j |j|j ¡dks²J ‚|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 )	NrT   r2   )Z
child_axisr   r   r&   )Zparent_axisrr   )r   r.   r   r   r(   r°   rj   rx   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 d   ƒ n1 s&w   Y  |j |j¡| j| j ks:J ‚|j	 |j¡|j|j ksIJ ‚t ƒ \} }}}tƒ  t
d||| j| j |j|j d}W d   ƒ n1 sow   Y  |j |j¡| j| j ksƒJ ‚|j	 |j¡|j|j ks’J ‚d S )Nr2   )Zparent_joint_posZchild_joint_pos)r.   r   r   r(   r)   rg   rY   rc   rd   rZ   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_trL   r.   r6   rR   rm   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,   r,   r,   r-   Ú<module>   sR    4
/9I(/L8 6d17=>(