a
    kº”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,   úV/var/www/auris/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,   ©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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: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   r   Ú
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     ÿ  $ÿ"" ""rO   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_Jr2   r   z PinJoint: J  parent: P  child: CÚP1ÚC1r%   r&   ÚJ1©Úparent_pointÚchild_pointÚ
joint_axisÚparent_interframeúq, uTrV   rW   ©rV   rW   rY   Úchild_interframerX   )&r   r   r   r   Únamer/   r0   Úcoordinatesr   rF   ÚkdesrH   rI   rX   r#   r(   rW   Úpos_fromÚ
masscenterr   rV   Ú_child_pointrY   r\   Ú__str__r   r'   r)   r   ÚzZ_joint_axisZ_parent_pointr.   Ú	locatenewÚvelÚexpressr   r   )r!   r$   ÚlÚmr<   r;   ZPjrR   rS   r*   rT   r   r    r+   rV   rW   r2   r,   r,   r-   Útest_pin_jointU   sp    þ  ÿ
þ  ,,ÿrj   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snJ ‚|j	 
|j	¡| |j ksŒJ ‚| |j¡tg d	¢d
t|ƒt|ƒgd
t|ƒ t|ƒggƒksÌJ ‚|j	 |j¡| | |j ksîJ ‚tdƒ}tdƒ}tdƒ}td| d}tdƒ\}}td||||||||j |jd	}|j|jjksTJ ‚| |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sBJ ‚|j	 
|j	¡||jj ksbJ ‚|j	 |j¡d
kszJ ‚|j	 |j¡||jj ksšJ ‚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sDJ ‚|j	 |j¡||j ksbJ ‚|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sVJ ‚|j	 |j¡|| |j ||j  ||j  ksŒJ ‚tdƒtdƒ }}tdƒtdƒ }}td||||||j ||j d}| |¡tdƒksêJ ‚|j	 
|j	¡||j ||j  ksJ ‚|j	 |j¡d
ks*J ‚d S )Núm lr$   r!   )ÚmassrZ   r2   ©r\   rW   Z	J_P_frame©r8   r   r   r   )rY   r\   rW   rX   ©r   r   r8   z	q1:4 u1:4c                 S   s   i | ]\}}|  t¡|“qS r,   )rH   rI   )Ú.0ZqiZuir,   r,   r-   Ú
<dictcomp>­   r5   z/test_particle_compatibility.<locals>.<dictcomp>r:   ÚP_frame)rY   rV   rX   ©r   r8   r   ÚC_frame)rY   r\   rV   rW   )r   r   r   r   r   r)   r\   rY   r]   ra   r`   Údcmr   r   r   rf   rd   rX   r(   r   r
   r   Úxreplacer   r   )ri   rh   rt   r!   r$   r<   r;   r2   rr   rA   rJ   rK   rC   rL   rM   Z	qdot_to_ur,   r,   r-   Útest_particle_compatibilityŽ   sš    ÿþ"
þþ"   ÿ(ÿ(ÿ

ÿ(ÿÿ
 ÿ


þ(rw   c                  C   s  t dƒ\} }tdƒ}tƒ & tdƒ}td| |d}W d   ƒ n1 sF0    Y  tdƒ\}}td||||||j d |j|ks„J ‚|jjdks”J ‚|j	 
|j	¡| |j ks²J ‚|j |j¡tg d	¢d
t|ƒt|ƒgd
t|ƒ t|ƒggƒksôJ ‚|j	 |j¡| | |j ksJ ‚d S )Nrk   r$   r!   ©rl   r#   rZ   r2   ©rW   rr   rn   r   )r   r   r   r   r   r   r)   r#   r]   ra   r`   ru   r   r   r   rf   rd   )ri   rh   rt   r!   r$   r<   r;   r,   r,   r-   Útest_body_compatibility×   s    ,þrz   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 u2rk   r   r    ÚBr$   r"   r!   ©r#   rl   ÚRrT   )rF   r^   rW   rX   ÚJ2r   ro   )r   r   r   r   r   r(   r#   rd   ru   r   r   r   r	   Ú
ang_vel_inr_   rH   rI   ra   rf   r)   )rA   rJ   rC   rL   ri   rh   r   r    r{   r$   ZPartPZPartRrT   r~   r,   r,   r-   Útest_pin_joint_double_pendulumè   sB    ÿÿÿÿ(þ$    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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    r{   r&   é   Úrodr|   Úplaterx   r$   r"   rT   )r^   rF   rW   rX   r~   )r^   rF   rV   rX   r   rs   ro   )r   r   r   r   r   rd   r)   ru   r   r   r   r   r_   rH   rI   ra   r`   rf   r(   )ZmAZmBZlAZlBÚhÚthetaÚphiÚomegaÚalphar   r    r{   ZlCr‚   rƒ   r$   rT   r~   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 )NrZ   r*   r+   c                      s   t dˆˆ ˆdS ©Nr2   ©rY   ©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   )rV   rW   rY   rX   g      à¿r&   é   éþÿÿÿr   )rV   rW   r\   rX   r8   r[   )r   r.   r   r   rG   r'   rd   Zorient_body_fixedr   r   r(   r)   r	   ru   r   r   r   r   r   r   ra   r`   rf   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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 )NrZ   Tr2   ©rY   r\   rX   r   ©r   éÿÿÿÿr   )r   r   r—   ©r—   r   r   c                      s(   t dˆˆ tˆƒˆj tˆƒˆj  dS ©Nr2   )rX   )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)   rX   ru   r   r   r   rd   r(   r   rG   Ú	Exception)r;   r    Úpinr,   )r$   r+   r   r!   r*   r<   r-   Útest_pin_joint_joint_axis‹  sZ    ÿ ÿÿ"ÿ
ÿ0ÿ
ÿ ÿ
ÿr¡   c            	      C   sP  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
sbJ ‚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ƒ   ¡ s J ‚| 
|¡ |¡ ¡ ||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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sLJ ‚d S )NrQ   r2   r   r   r˜   r&   rm   rn   ©rY   rV   rs   r8   ©rV   rW   r\   r:   r‘   r   é   é   úm n©rV   rW   r\   rY   éüÿÿÿr’   )r   r.   r   r(   Úangle_betweenrg   ru   r   r   r   r   Ú	magnituder   ra   r`   r	   rf   r)   Zcrossrd   r   rv   r   r   r   Údot)	r<   r;   r   r    r!   r$   Zangleri   Ú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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(   rd   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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 uTr2   )rY   r\   r   rn   r•   c                      s   t dˆˆ ˆˆj dS r™   )r   rd   r,   rš   r,   r-   r4   x  r5   z%test_pin_joint_axis.<locals>.<lambda>)r   r.   r   rX   r(   r   r   r   rd   ru   r   rG   )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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 )Nr2   ©rV   Z	J_P_jointÚp)rV   rW   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   ry   rŒ   r,   ©r$   r!   rW   r,   r-   r4   “  r5   c                      s   t dˆˆ ˆdS r·   rŒ   r,   r¸   r,   r-   r4   –  r5   )r.   r   r)   rd   rV   r]   r`   ra   rW   re   r   r6   r   rG   r   )r    ÚjointrV   r,   )r$   r   r!   rW   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_framer8   r2   r‹   r   r<   c                      s   t dˆˆ ˆdS rŠ   rŒ   r,   )r$   r!   rY   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'   rd   r   rY   r   r\   r   r   rG   )r   r    r¹   r<   r,   )r$   r!   r\   rY   r-   Útest_locate_joint_frame™  s(    ÿÿr»   c               	   C   sH  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 ‚|j |j¡dksVJ ‚|j |j¡dksnJ ‚| ¡ 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s6J ‚|j |j¡| |jj ksXJ ‚|j |j¡| |j ||j  |	|j  ksŒJ ‚|j |j¡||jj ks¬J ‚|j |
¡tdƒksÆJ ‚|j |j¡dksÞJ ‚|j |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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sDJ ‚d S )	Núq_S, u_Sr   r   z&PrismaticJoint: S  parent: P  child: CrP   r%   r&   rU   )r.   r   r   r]   r/   r0   r^   r   rF   r_   rH   rI   rX   r#   r(   rW   r`   ra   r   rV   rf   r   rc   r   r   r'   r)   r   rd   )rN   r!   r$   r<   r;   r   r   r    rh   ri   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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snJ ‚|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sRJ ‚|j 	|j¡| d |j |j kszJ ‚|j 	|j¡ |¡| tdƒd  d |j tdƒd |j  tdƒd |j    
¡ dksÚJ ‚|j |¡||j ksöJ ‚|j |¡ |¡tdƒ| d |j tdƒ| d |j  tdƒ| d |j    
¡ sTJ ‚| |¡dkshJ ‚| |¡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sÂJ ‚|j 	|j¡|tdƒ|  d  |j tdƒ|  d |j  tdƒ|  d |j  ||j    |¡ 
¡ dk	s4J ‚|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
s€J ‚| |¡dk
s”J ‚| |¡dk
s¨J ‚d S )Nr¼   r   r   r   r˜   r–   ro   rm   rn   r¢   rs   r£   r&   r8   r:   r‘   r¦   r§   r’   )r   r.   r   r(   r©   rg   ru   r   ra   r`   r	   rf   r   r)   r   rd   r   r   )r<   r;   r   r    r!   r$   ri   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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_Jr2   r   z(CylindricalJoint: J  parent: P  child: Cz
q0:2, u0:2úl, mTr&   )Úrotation_coordinateÚrotation_speedÚtranslation_speedrV   rW   rY   r\   rX   ) r.   r   r   r]   r/   r0   r^   r   rF   rÀ   Ztranslation_coordinaterÁ   rÂ   r_   rH   rI   rX   r(   rW   r`   ra   r   rV   rb   rf   r   rY   r\   rc   r   r)   rd   )r   r    r!   r$   Úq0_defÚq1_defÚu0_defÚu1_defÚCjr@   rA   rB   rC   rh   ri   r*   r+   r,   r,   r-   Útest_cylindrical_jointK  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_Jr2   r   z#PlanarJoint: J  parent: P  child: Cú
q0:3, u0:3r¿   T)rÀ   Úplanar_coordinatesÚplanar_speedsrV   rW   rY   r\   ) r.   r   r   r]   r/   r0   r^   r   rF   rÀ   rÊ   rÁ   rË   r_   rH   rI   Úrotation_axisr(   Zplanar_vectorsr)   rd   rW   r`   ra   r   rV   rf   r   rY   r\   rc   r   )r   r    r!   r$   rÃ   rÄ   Zq2_defrÅ   rÆ   Zu2_defrÇ   Zr_P_Cr@   rA   rJ   rB   rC   rL   rh   ri   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:3r2   r:   )rV   rW   rY   r\   r‘   r   r&   r   g      Ð?r8   g      è?)r   r   r.   r   rd   r)   r   rÌ   rg   Ú	normalizer©   r   ru   rv   r
   r   r   r   r	   r   )r@   rA   rJ   rB   rC   rL   Ú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<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/   r0   r^   r   rF   r_   rH   rI   rW   r`   ra   r   rV   rf   r   r   r   r(   r)   rd   rc   Ú	_rot_typeÚ
_rot_orderZ_amounts)r   r    r!   r$   r@   rA   rJ   rB   rC   rL   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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:3r8   r   )r^   rF   )r   r   r   )r   r.   r   r^   r   rF   r_   r   r   r   r(   r)   rd   )r@   rA   rJ   rB   rC   rL   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@   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   rJ   rK   rB   rC   rL   r,   r-   r4     s    ÿc                
      s&   t dˆˆ tˆˆˆgƒtˆˆˆˆgƒƒS rÙ   rÚ   r,   )	r$   r!   r@   rA   rJ   rB   rC   rL   Úu4r,   r-   r4     s    ÿ)r   r.   r   r^   r   rF   r   rG   )	Zq0sZq1sZq2sZu0sZu1sZu2sr   r    r   r,   )
r$   r!   r@   rA   rJ   rK   rB   rC   rL   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 ‚t| |¡ |¡| ƒtd
dƒks‚J ‚t|j |¡ |¡ƒ|ks¢J ‚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s4J ‚t| |¡|
| ƒ ƒtd
ƒksXJ ‚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Ô   ©r^   rF   rV   rW   rY   r\   Úrot_typeÚ	rot_orderrÓ   r:   r8   ©	r^   rF   rV   rW   rY   r\   rß   Zamountsrà   c                    s"   |   ˆ ˆˆˆ ˆˆˆˆˆˆˆˆi¡S r®   ©rv   ©Úexpr©r@   rA   rJ   rB   rC   rL   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)   rd   rÕ   ÚupperrÖ   r	   ru   r   r   r¶   ra   rf   ©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&   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(   rd   r)   rÕ   ræ   rÖ   r	   ru   r   r   r¶   ra   rf   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s6J ‚|j|ksDJ ‚|jtƒ ksTJ ‚|jtƒ ksdJ ‚|jtddg ƒ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¡tdƒksþJ ‚|j |j
¡tdƒksJ ‚|j
 |j
¡dks2J ‚|j
 |j
¡dksJJ ‚| ¡ 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sZJ ‚|j |¡tdƒkstJ ‚|j
 |j
¡dksŒJ ‚|j
 |j
¡dks¤J ‚|j|jks¶J ‚tƒ  t||ƒ W d   ƒ n1 sÞ0    Y  d S )
NÚWr8   r   r:   z!WeldJoint: W  parent: P  child: CrP   r%   r&   )rV   rW   rY   )r.   r   r]   r/   r0   r^   r   rF   r_   ÚTr#   ru   r
   rW   r`   ra   r   rV   rf   r   rc   r   r   r'   r)   r   r(   rd   r   r   )	rN   r!   r$   rì   r   r    rh   ri   r*   r,   r,   r-   Útest_weld_jointŒ  sF    ÿ  *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 )	NrQ   r2   )Z
child_axisr   r˜   r&   )Zparent_axisro   )r   r.   r   r   r(   r©   rg   ru   r   r   r   r   rª   r   r   r)   r   )r<   r;   r   r    r!   r$   r,   r,   r-   Ú!test_deprecated_parent_child_axis´  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 )Nr2   )Zparent_joint_posZchild_joint_pos)r.   r   r   r(   r)   rd   rV   r`   ra   rW   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_trI   r.   r7   rO   rj   rw   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-   Ú<module>   sP   4
/9I(/L8 6d17=>(