
    MZd+                        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jL                  Z'd(dZ(d Z)d Z*d Z+d Z,d Z-d Z.d Z/d Z0d Z1d Z2d Z3d Z4d Z5d Z6d Z7d Z8d Z9d Z:d  Z;d! Z<d" Z=d# Z>d$ Z?d% Z@d& ZAy'))    )
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_sympyTc                 :   t        d      }t        d      }t        d|      }t        d|      }| rdt        d      t        d      }}|j                  ||j                  t               |j                  ||j
                  t         dz         ||||||fS ||||fS )	NNAPframeCP_intC_int   )r   r   orient_axisxr   y)
interframer   r    r!   r$   PintCints          J/usr/lib/python3/dist-packages/sympy/physics/mechanics/tests/test_joint.py_generate_bodyr/      s    sAsASASA#G,nW.EdACC$ACC"q)!Q4%%aA:    c                  \     t        d      t        d       t        t         fd       y )Nparentchildc                      t        d       S NJr   r3   r2   s   r.   <lambda>ztest_Joint.<locals>.<lambda>$   s    eC7 r0   )r   r   	TypeErrorr7   s   @@r.   
test_Jointr:   !   s     (^FME
978r0   c            	         t        d      \  } }}}t        d      \  }}}}}}	}
t        d      \  }}}}t               \  }}t        d      j                  | d      t	        | g      k(  sJ j                  |gd      t	        |g      k(  sJ j                  |gdd      t	        |g      k(  sJ j                  d d      t	        |g      k(  sJ j                  d gd      t	        |g      k(  sJ j                  d d gd      t	        |g      k(  sJ j                  d d      t	        ||g      k(  sJ j                  g d      t	        ||g      k(  sJ j                  g dd      t	        ||g      k(  sJ j                  d |gdd      t	        ||g      k(  sJ j                  d dd      t	        ||g      k(  sJ j                  d dd	      t	        |g      k(  sJ j                  g dd	      t	        |||	g      k(  sJ j                  d dd
      t	        |g      k(  sJ j                  g dd	dd
      t	        |	g      k(  sJ j                  g dd      t	        ||g      k(  sJ t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t               \  }}t        dj                  t                     t               \  }}t        t        fd       t        t        fd       y )Nzq u q_J u_Jzq0:4_J u0:4_Jz	q0:4 u0:4r6      r'   )offset   uT)number_singleqc                  ,     j                  gd      S )Nr<   _fill_coordinate_listr6   q0q1s   r.   r8   z,test_coordinate_generation.<locals>.<lambda>G       q66BxC r0   c                  0     j                  d gdd      S )Nr'   r?   rC   )r6   u0u1s   r.   r8   z,test_coordinate_generation.<locals>.<lambda>H   s    q66B~q#N r0   c                  ,     j                  gd      S )Nr>   rC   rE   s   r.   r8   z,test_coordinate_generation.<locals>.<lambda>I   rH   r0   c                  >     j                  t        d      gd      S )NrG   r'   )rD   r   )r6   rF   s   r.   r8   z,test_coordinate_generation.<locals>.<lambda>K   s    a55r74=6I1M r0   c                  2     j                  z   gd      S )Nr'   rC   rE   s   r.   r8   z,test_coordinate_generation.<locals>.<lambda>L   s    a55rBwmQG r0   c                  &    t        d d d g      S r5   r   )r$   r!   q1js   r.   r8   z,test_coordinate_generation.<locals>.<lambda>R   s    ~c1a#tT9JK r0   c                  (    t        d g      S )Nr6   )speedsrP   )r$   r!   rJ   rK   s   r.   r8   z,test_coordinate_generation.<locals>.<lambda>S   s    ~c1aRM r0   )
r   r/   r   rD   r	   r   
ValueErrorr9   difft)rA   r?   qjujq0jq2jq3ju0ju1ju2ju3jq2q3u2u3_r$   r6   r!   rF   rG   rQ   rJ   rK   s                   @@@@@@@@r.   test_coordinate_generationre   '   sq   !-0LAq"b-;O-L*Cc3S#s%3K%@"BBBB!JAq!QaA""1a(FA3K777""A3*faSk999""A3!"4sCCC""4+vrd|;;;""D61-===""Bd#3Q76	S#<    ""4+vsCo/FFFF""2q)VS#sO-DDDD""2q"3vsCo7NNNN""Bb>1Q"?6	S"D   ""41"5c
9KKKK""4C0FB4L@@@""2q#.&#sC2IIII""4$"?63%=PPP""2q#q$763%=HHH""2q#.&#sC2IIII
:CD
:NO
:CD
9MN
9GH!JAq!QS!QBGGAJ'!JAq!Q
:KL
:MNr0   c                  @   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(  sJ |j                  t        |g      k(  sJ |j                  t        ||j                  t              z
  g      k(  sJ |j                  | j                  j                  k(  sJ |j                   j#                  |j$                        t'        d      k(  sJ |j(                  j#                  | j$                        t'        d      k(  sJ |j(                  j#                  |j*                        t'        d      k(  sJ |j$                  j#                  | j$                        t'        d      k(  sJ |j,                  | j                  k(  sJ |j.                  |j                  k(  sJ |j1                         dk(  sJ t        d      }t        d	      }t3        d
      }	|	j5                  |j                  |j6                  t8        dz         t        d||||j                  j                  z  ||j                  j6                  z  |j                  j:                  |	      }
|
j<                  |j                  j:                  k(  sJ |
j*                  j#                  |j$                        ||j                  j6                  z  k(  sJ |
j>                  j#                  |j$                        ||j                  j                  z  k(  sJ |
j>                  j#                  |
j*                        t'        d      k(  sJ |j$                  j#                  |j$                        | |j                  j                  z  ||j                  j6                  z  z   k(  sJ |
j,                  |	k(  sJ |
j.                  |j                  k(  sJ t        d      \  }}tA        d      \  }}} }}	}| j$                  jC                  d|j                  |j6                  z         }|j$                  jC                  d|j6                  |j:                  z         }t        d| ||||||	||j:                  
      }|j                  |j:                  k(  sJ |j(                  jE                  |      dk(  sJ |j(                  |k(  sJ |j                   |k(  sJ |j                   j#                  | j$                        |j                  |j6                  z   k(  sJ |j(                  j#                  |j$                        |j6                  |j:                  z   k(  sJ |j$                  j#                  | j$                        |j                  |j6                  z   |j6                  z
  |j:                  z
  k(  sJ |j$                  jE                  |      jG                  |      |tI        |      z  |tK        |      z  z
  |j                  z  | tI        |      z  |tK        |      z  z
  |j6                  z  z   k(  sJ |j,                  |	k(  sJ |j.                  |k(  sJ y )Nr!   r$   l mq_J, u_Jr6   r   z PinJoint: J  parent: P  child: CP1C1r%   r'   J1parent_pointchild_point
joint_axisparent_interframeq, uTrm   rn   rm   rn   rp   child_interframero   )&r   r   r   r   namer2   r3   coordinatesr	   rS   kdesrU   rV   ro   r#   r)   rn   pos_from
masscenterr   rm   _child_pointrp   rs   __str__r   r(   r*   r   z_joint_axis_parent_pointr/   	locatenewvelexpressr   r   )r!   r$   lmrA   r?   Pjri   rj   r,   rk   r   r    r-   rm   rn   r6   s                    r.   test_pin_jointr   V   sX   S	AS	A5>DAq*%DAq	#q!	B77c>>99>>88q==>>VQC[(((99s###77fa!&&)m_----==AGGII%%%>>""1<<0F1I===??##ALL1VAY>>>??##BOO4q	AAA<<  .&);;;177***!'')))::<====	dB	dB'"DRXXrttR!V,	$BQrxxzz\

lrxxzz$(
*B >>RXXZZ'''??##BMM2a"((**nDDD$$R]]3q288::~EEE$$R__5BBBMM""2==1BrxxzzMAbhhjjL() * )4'''"((***&!DAq+D1Aq!Qd<<)).!##)DL,,((acc	BKaAq|(D"&133	8A <<133>>a A%%%>>\)))==K'''==!!!,,/1339<<<>>""1<<0ACC!##I===<<  .!##)acc/ACC2GGGG<<A&&q)a#a&j1s1v:.E-LBQK!c!f*$P, ., , , ,$&&&%%%r0   c            	         t        d      \  } }t        d      \  }}t        d      \  }}t        d      }t        d      }t        d      }t        d|      }	t        d	||
      }
t        d||
      }t	        d|	|
|| | |j
                  z  |	j                  j                        }t	        d|
|||| |j
                  z  |
j                  j                        }|j                  |      t        t        |       t        |        dgt        |       t        |       dgg dg      k(  sJ |j                  |      t        t        |      t        |       dgt        |      t        |      dgg dg      k(  sJ t        |j                  |            t        t        | |z         t        | |z          dgt        | |z         t        | |z         dgg dg      k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      ||j                  z  ||j                  z  z   k(  sJ |j                  t        || j                  t               z
  g      k(  sJ |j                  t        ||j                  t               z
  g      k(  sJ |
j"                  j%                  |      ||z  |j&                  z  k(  sJ |j"                  j%                  |      ||z  |j&                  z  k(  sJ |j"                  j%                  |      ||z  |j&                  z  |||z   z  |j&                  z  z   k(  sJ y )Nzq1 q2zu1 u2zm lr   r    Br$   r"   r!   r#   massRrk   )rS   ru   rn   ro   J2r   r   r   r<   )r   r   r   r   r   r)   r#   r{   dcmr	   r   r   r
   
ang_vel_inrv   rU   rV   rx   r   r*   )rG   r`   rK   rb   r   r   r   r    r   r$   PartPPartRrk   r   s                 r.   test_pin_joint_double_pendulumr      s    G$FBG$FB5>DAqsAsAsASAAA&EAA&E	$5 bf
<B	$uRR bf
@B 558vB#b'15 #BR!4iA B B B B558vB#b'15 #BR!4iA B B B BAEE!H%#b2g,R"Wq1Q25b2g,BGa1P1:1< *= = = =
 <<?b133h&&&<<?b133h&&&<<?b133hacc1111 77fb2771:o.////77fb2771:o.//// "ad133h..."ad133h..."ad133hBGQSS&@@@@r0   c            
         t        d      \  } }}}}t        d      \  }}}}t        d      }	t        d      }
t        d      }||dz  z
  dz  }|dz  |dz  z   }t        d|
| 	      }t        d
||      }t        d|	      }t	        d||||||
j
                  z  |	j                        }t	        d||||||
j
                  z  |
j
                        }|
j                  |	      t        t        |      dt        |       gg dt        |      dt        |      gg      k(  sJ |
j                  |      t        t        |      t        |       dgt        |      t        |      dgg dg      k(  sJ |j                  |	      t        t        |      t        |      z  t        |      t        |       t        |      z  gt        |       t        |      z  t        |      t        |      t        |      z  gt        |      dt        |      gg      k(  sJ |
j                  |	      ||	j                  z  k(  sJ |
j                  |      | |
j
                  z  k(  sJ |	j                  |      | |	j                  z  ||
j
                  z  z
  k(  sJ |j                  t        ||j                  t              z
  g      k(  sJ |j                  t        ||j                  t              z
  g      k(  sJ |j                  j!                  |j                        ||
j
                  z  k(  sJ |j                  j!                  |j                        | |
j
                  z  k(  sJ |j                  j#                  |	      |dz  |dz  z
  |z  |
j$                  z  k(  sJ |j                  j#                  |	      |dz  |dz  z
  |z  |dz  |dz  z   |z  z   |
j$                  z  k(  sJ y )NzmA, mB, lA, lB, hztheta phi omega alphar   r    r   r'      rodr   plate)r   r#   r$   r"   rk   )ru   rS   rn   ro   r   )ru   rS   rm   ro   r   r   r<   r   r   )r   r   r   r   r   r{   r*   r   r	   r   r   r   rv   rU   rV   rx   rw   r   r)   )mAmBlAlBhthetaphiomegaalphar   r    r   lCr   r   r$   rk   r   s                     r.   test_pin_joint_chaos_pendulumr      s    34BBA-.EFE3usAsAsA
q1u*	B
Q$1*B
uAB
'Cr+ESA	$3E% f
6B	$UE!!##v!##
7B 558vE
AE
{;( #E
As5z:< = = = = 558vC3s8)Q7 #C#c(A6(* + + + + 558v	S#e*	c#hUCH(<=
c(3u:	s3xS#e*)<=	UQE
#% & & & & <<?eACCi'''<<?ufQSSj(((<<?ufQSSj594444 77feejjm34555577fechhqk123333 <<  0BqssF:::>>""5#3#34"qss
BBB >>a QqS2a4Z$6qss$::::"!bd
E'9()!bd
E'9(:;<33'? ? ? ?r0   c                     t        d      \  } }t               \  }}t        d      t        d      ct        t        fd       t        t        fd       j                  ||j                  |        j                  ||j                  |        t        t        fd       t        t        fd       t               \  }}t        d      j                  |t        dz  t        t        d	z  fd
       t        d| ||j                  j                   j                  	       t        |j                  |            t        dt        d	      t!        |       z  dz  t        d	       t#        |       z  dz  gt        d      dz  t        d      dt#        |       z  t!        |       z   z  dz  t        d      t#        |        dt!        |       z  z   z  dz  gt        d      dz  t        d      dt#        |       z  t!        |       z   z  dz  t        d       t#        |       dt!        |       z  z   z  dz  gg      z
  t%        d	      k(  sJ |j'                  |      |j                  z  k(  sJ j(                  j+                  j(                        |j                  |j                  z   k(  sJ j(                  j-                  |      ||j                  z  k(  sJ j(                  j-                        t/        d      k(  sJ j(                  j-                        ||j                  z  k(  sJ t               \  }}t        d      j                  |dt        z  d	z  t         t        dz  fd
       t        d| ||j                   j                  j                  j                  z   	       t        |j                  |            t        t        d       t#        |       z  dz  t        d	       t!        |       dz
  z  dz  t!        |       dz  z
  t1        d      dz  z
  t        d	      t!        |       dz   z  dz  t!        |       dz  z
  t1        d      dz  z   gt!        |       t        d      t        d      z   t#        |        z  dz  t        d       t        d      z   t#        |       z  dz  gt        d      t#        |       z  dz  t        d	      t!        |       dz   z  dz  t!        |       dz  z   t1        d      dz  z
  t        d	      dt!        |       z
  z  dz  t!        |       dz  z   t1        d      dz  z   gg      k(  sJ |j'                  |      t        d      |z  dz  |j                  z  t        d      |z  dz  |j                  z  z   k(  sJ j(                  j+                  j(                        |j                   |j                  z
  k(  sJ j(                  j-                  |      j3                         t        d       t        d      z
  |z  dz  |j                  z  t        d       t        d      z   |z  dz  |j                  z  z   k(  sJ j(                  j-                        t/        d      k(  sJ t               \  }}t        d      t        d      cj                  |t         dz  t        t        dz  fd
       j                  |dt        z  d	z  t         t        dz  fd
       t        d| ||j                  |j                  z
  j                   j                  j                  z   
       t        |j                  |            t        t!        |       t        d      t        d      z   t#        |        z  dz  t        d       t        d      z   t#        |       z  dz  gt        d       t#        |       z  dz  t        d	       t!        |       dz   z  dz  t!        |       dz  z
  t1        d      dz  z   t        d	      t!        |       dz
  z  dz  t!        |       dz  z
  t1        d      dz  z
  gt        d      t#        |       z  dz  t        d	      t!        |       dz
  z  dz  t!        |       dz  z   t1        d      dz  z   t        d	       t!        |       dz   z  dz  t!        |       dz  z   t1        d      dz  z
  gg      k(  sJ |j'                  |      t        d      |z  dz  j                  z  t        d      |z  dz  j                  z  z   k(  sJ j(                  j+                  j(                        |j                  |j                  z
  |j                  z   k(  sJ t        d       t        d      z   |z  dz  |j                  z  }j(                  j-                  |      j3                         |k(  sJ j(                  j-                        j3                         |k(  sJ j(                  j-                        t/        d      k(  sJ y )Nrq   r,   r-   c                  "    t        d       S Nr6   rp   r   r$   r!   r,   s   r.   r8   z+test_pin_joint_interframe.<locals>.<lambda>       xQTJ r0   c                  "    t        d       S Nr6   rs   r   r$   r-   r!   s   r.   r8   z+test_pin_joint_interframe.<locals>.<lambda>       xQDI r0   c                  "    t        d       S r   r   r   s   r.   r8   z+test_pin_joint_interframe.<locals>.<lambda>   r   r0   c                  "    t        d       S r   r   r   s   r.   r8   z+test_pin_joint_interframe.<locals>.<lambda>   r   r0   r   r>   xyzr6   )rm   rn   rp   ro   g      r'      r   )rm   rn   rs   ro   r<   rr   )r   r/   r   r   rT   r(   r{   orient_body_fixedr   r   r)   r*   r
   r   r	   r   r   r   r   r   rx   rw   r   r   r   simplify)	rA   r?   r   r    N_v_Cr$   r-   r!   r,   s	        @@@@r.   test_pin_joint_interframer      s#	   &!DAq!JAq!Q')?JD$
:JK
:IJQQQQ
:JK
:IJ!JAq!Q&!D1rAvr262E:S!Q1133QSSD#8AEE!H%	a3q6!A%Qx#a&'81'<=	a1d1gSVc!f!459	aSVGa#a&j(	)A	-	/	a1d1gc!fs1v!56:
q'SVa#a&j(	)A	-	/	00 )1 1
 5:!H= = = <<?a$&&j(((<<  .!##);;;<<A!acc')))<<D!VAY...<<D!QW,,,!JAq!Q&!D1q2vzB3Q7?S!Q1ACC4QSS"qssQSSy:AEE!H%
q'CF	Q	
q'SVaZ	 1	$s1vz	1AaD1H	<	aCFQJ	!	#c!fqj	01Q4!8	;	= 
Q$q'DG#Aw.27(T!W
A	&	*	,	a3q6	A		aCFQJ	!	#c!fqj	01Q4!8	;	aAAJ	!	#c!fqj	01Q4!8	;	=1> *? ? ? ? <<?d1gkAo3d1gkAo6KKKKK<<  .ACC%!##+===<<A'')	a47a. "#.$&'cc.*Qx$q'!Q-')*-+-.SS-1.1 1 1 1 <<D!VAY...!JAq!Q')?JD$1sQwBF3U;1q2vzB3Q7?S!Q11339133$#d) AEE!H%	Q$q'DG#Aw.27(T!W
A	&	*	,
q'CF	Q	
q'SVaZ	 1	$s1vz	1AaD1H	<	aCFQJ	!	#c!fqj	01Q4!8	;	= 
a3q6	A		aCFQJ	!	#c!fqj	01Q4!8	;
q'SVaZ	 1	$s1vz	1AaD1H	<	>1? *@ @ @ @ <<?d1gkAo6	:::VV:    <<  .!##)acc/AAA1gXQ1$q(133.E<<A'')U222<<D!**,555<<D!VAY...r0   c            
         t        d      \  } t        d      \  }t        d| j                        }|j                  j                  k(  sJ j                  |      t        t              dt              gg dt              dt               gg      k(  sJ t        d      \  }t        d| j                        }|j                  j                  k(  sJ j                  |      t        t               dt              gg dt              dt              gg      k(  sJ t               \  }t        d| j                  j                   j                        }|j                  j                  k(  sJ j                  |      t        t               t               dgt               t              dgg dg      k(  sJ t               \  }t        d| j                  j                   j                        }|j                  j                  k(  sJ j                  |      t        g ddt              t              gdt              t               gg      k(  sJ t        d      \  }t        t        fd	       t        t        fd
       t        t        fd       t        t        fd       t        t        fd       t        d      \  }t        dj                  j                  z          t        t        fd       t        t        fd       y )Nrq   Tr6   rp   rs   ro   r   r   r   )r   r   r   r   r   r   c            	          t        d t              j                  z  t              j                  z  z         S Nr6   )ro   )r   r   r)   r   r*   r$   r   r!   rA   s   r.   r8   z+test_pin_joint_joint_axis.<locals>.<lambda>Q  s3    xQ36q6ACC<#a&133,3N P r0   c                  6    t        d  j                        S r   r   r)   r$   r!   s   r.   r8   z+test_pin_joint_joint_axis.<locals>.<lambda>T  s    xQaccB r0   c                  P    t        d j                   j                  z         S r   r   r)   r*   r   s   r.   r8   z+test_pin_joint_joint_axis.<locals>.<lambda>V  s    xQaccACCiH r0   c            	      T    t        d j                   j                  z         S Nr6   r   r   r$   r-   r!   r,   s   r.   r8   z+test_pin_joint_joint_axis.<locals>.<lambda>W  s&    xQTD66ACC< ! r0   c            	      T    t        d j                  j                  z         S r   r   r   s   r.   r8   z+test_pin_joint_joint_axis.<locals>.<lambda>Z  s&    xQTD33< ! r0   c            
      8    t        d t        d            S )Nr6   r   r   )r   r   r   s   r.   r8   z+test_pin_joint_joint_axis.<locals>.<lambda>b  s    hQTD!9 r0   c            	      T    t        d j                  j                  z         S r   )r   r*   r   s   r.   r8   z+test_pin_joint_joint_axis.<locals>.<lambda>e  s&    hQTD33<! r0   )r   r/   r   r*   ro   r   r	   r   r   r{   r)   r   rT   	Exception)	r?   r    pinr$   r-   r   r!   r,   rA   s	      @@@@@@r.   test_pin_joint_joint_axisr   2  sA   &!DAq+D1Aq!Qd
31ad$(QSS:C>>QSS   558vA3q62J #ACF735 6 6 6 6 ,D1Aq!Qd
31ad$(TVV=C>>TVV###558vQCF3Z #A3q624 5 5 5 5  !JAq!Q
31aacc%&SSDQSS:C>>QSS   558vQ#a&!4AwA6J)+ , , , ,!JAq!Q
31aacc%&SSDQSS:C>>QSS   558vzAs1vs1v+> !3q6CF735 6 6 6 6 ,D1Aq!Qd
: P Q :BC
:HI
: ! " : ! " ,D1Aq!QdS!Q$& 9   9 ! "r0   c                  /   t        d      \  } }t               \  }}}}t        d|||j                          |j                   j	                  |j                        dk(  sJ |j                  j                  |       |j                  k(  sJ |j                  |      t        g ddt        |        t        |        gdt        |        t        |       gg      k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      j                         t        |dz        k(  sJ |j                  j                  |j                        dk(  sJ |j                  j                  |j                        j                  |      j                         dk(  sJ |j                  j!                  |      dk(  sJ t               \  }}}}t        d|||j"                  |j                         |j"                  j	                  |j                        dk(  sJ |j"                  j                  |      |j                  k(  sJ |j                  |      t        dt        |        t        |        gg ddt        |        t        |       gg      k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      j                  |      ||j"                  z  k(  sJ |j                  |      j                         t        |dz        k(  sJ |j                  |      j%                  |j"                        dk(  sJ |j                  j!                  |      ||j&                  z  k(  sJ |j                  j                  |j                        |j                   k(  sJ |j                  j                  |j                        j                  |      j                         t        |       |j"                  z  t        |       |j&                  z  z   k(  sJ |j                  j!                  |      j	                  |j                        t(        dz  k(  sJ t               \  }}}}t        d|||j"                  |j                  	       |j"                  j	                  |j                        dk(  sJ |j"                  j                  |      |j                  k(  sJ |j                  |      t        g d
t        |        dt        |       gt        |       dt        |       gg      k(  sJ |j                  |      ||j"                  z  k(  sJ |j                  |      j                  |      ||j                  z  k(  sJ |j                  |      j                         t        |dz        k(  sJ |j                  |      j	                  |j                        }|j+                  |di      dk(  sJ |j                  j!                  |      dk(  sJ |j                  j                  |j                        |j                  k(  sJ t               \  }}}}t        d|||j                  |j                  |j                  |j"                  z          t-        |j                  j	                  |j                  |j"                  z               dk(  sJ |j                  |j"                  z   j                  |      j                         t        d      |j                  z  k(  sJ t/        |j                  |            t        t        d      dz  t        d       t        |       z  dz  t        d       t        |       z  dz  gt        d      dz  t        d      t        |       z  dz  t        d      t        |       z  dz  gdt        |        t        |       gg      k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      j                  |      j                         ||j                  z  ||j"                  z  z   t        d      z  k(  sJ |j                  |      j                         t        |dz        k(  sJ |j                  |      j	                  |j                  |j"                  z         }|j+                  |di      dk(  sJ |j                  j!                  |      j                         ||j&                  z  t        d      z  k(  sJ |j                  j                  |j                        |j                  |j                  z
  k(  sJ |j                  j                  |j                        j                  |      j                         dt        d      dz  z
  |j                  z  t        d      t        |       z  dz  |j"                  z  z   t        d      t        |       z  dz  |j&                  z  z   k(  sJ |j                  j!                  |      j                  |      j                         t        d       |z  t        |       z  dz  |j"                  z  t        d      |z  t        |       z  dz  |j&                  z  z   k(  sJ |j                  j!                  |      j	                  |j                        t(        dz  k(  sJ t               \  }}}}t        d|||j                  |j                  |j                  |j"                  z   |j&                  z
         t-        |j                  j	                  |j                  |j"                  z   |j&                  z
              dk(  sJ |j                  |j"                  z   |j&                  z
  j                  |      j                         t        d      |j                  z  k(  sJ t/        |j                  |            t        t        d      dz  t        d       t        | t(        dz  z         z  dz  t        d      t        | t(        dz  z         z  dz  gt        d      dz  t        d      t        | t(        dz  z         z  dz  t        d      t        | t(        dz  z         z  dz  gt        d       dz  t        d      t        | dt(        z  dz  z         z  dz  t        d      t        | dt(        z  dz  z         z  dz  gg      k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      j                  |      j                         ||j                  z  ||j"                  z  z   ||j&                  z  z
  t        d      z  k(  sJ |j                  |      j                         t        |dz        k(  sJ |j                  |      j	                  |j                  |j"                  z   |j&                  z
        }|j+                  |di      dk(  sJ |j                  j!                  |      j                         ||j"                  z  ||j&                  z  z   t        d      z  k(  sJ |j                  j                  |j                        |j                  |j                  z
  k(  sJ |j                  j                  |j                        j                  |      j                         dt        d      dz  z
  |j                  z  t        d      t        | t(        dz  z         z  dz  |j"                  z  z   t        d      t        | t(        dz  z         z  dz  |j&                  z  z
  k(  sJ |j                  j!                  |      j                  |      j                         t        d      |z  t        | t(        dz  z         z  dz  |j"                  z  t        d      |z  t        | t(        dz  z         z  dz  |j&                  z  z   k(  sJ |j                  j!                  |      j	                  |j                        t(        dz  k(  sJ t               \  }}}}t1        d      \  }}t        d||||j                  z  ||j                  z  |j                  |j"                  z   |j&                  z
  |j                  |j"                  z
  |j&                  z          |j                  |j"                  z
  |j&                  z   j	                  |j                  |j"                  z   |j&                  z
        }t-        |      dk(  sJ |j                  |j"                  z
  |j&                  z   j                  |      j                         dt        |       z  dz  t3        d      dz  z
  |j                  z  t3        d      dz  dt        | t(        dz  z         z  dz  z
  |j"                  z  z   dt        | t(        dz  z         z  dz  t3        d      dz  z
  |j&                  z  z   k(  sJ t/        |j                  |            t        t3        d      dz  dt        |       z  dz  z
  dt        | t(        dz  z         z  dz  t3        d      dz  z
  dt        | t(        dz  z         z  dz  t3        d      dz  z   gdt        | t(        dz  z         z  dz  t3        d      dz  z   dt        |       z  dz  t3        d      dz  z
  dt        | t(        dz  z         z  dz  t3        d      dz  z   gdt        | t(        dz  z         z  dz  t3        d      dz  z
  dt        | t(        dz  z         z  dz  t3        d      dz  z   dt        |       z  dz  t3        d      dz  z
  gg      k(  sJ |j                  |      ||j                  z  ||j"                  z  z
  ||j&                  z  z   t        d      z  k(  sJ |j                  |      j                  |      j                         ||j                  z  ||j"                  z  z   ||j&                  z  z
  t        d      z  k(  sJ |j                  |      j                         t        |dz        k(  sJ |j                  |      j	                  |j                  |j"                  z   |j&                  z
        }|j+                  |di      dk(  sJ |j                  j!                  |      j                         t        d      |z  |z  dz  |j"                  z  t        d      |z  |z  dz  |j&                  z  z   k(  sJ |j                  j                  |j                        ||j                  z  ||j                  z  z
  k(  sJ |j                  j                  |j                        j                  |      j                         ||dt        |       z  dz
  z  dz  z   |j                  z  |dt        | t(        dz  z         z  dz   z  dz  |j"                  z  z   |dt        | t(        dz  z         z  dz   z  dz  |j&                  z  z
  k(  sJ |j                  j!                  |      j                  |      j                         d|z  |z  t        |       z  dz  |j                  z  d|z  |z  t        | t(        dz  z         z  dz  |j"                  z  z   d|z  |z  t        | t(        dz  z         z  dz  |j&                  z  z   k(  sJ |j                  j!                  |      j5                  |j                  |j"                  z
  |j&                  z         j                         dk(  sJ y )Nrh   r6   r   r   r   r'   rs   rn   r<   r   r   rp   rm   r   r<   rm   rn   rs   r>   r   r         m nrm   rn   rs   rp   r   )r   r/   r   r)   angle_betweenr   r   r	   r   r   r   	magnituder   rx   rw   r   r   r*   crossr{   r   xreplacer   r
   r   r   dot)	rA   r?   r   r    r!   r$   angler   ns	            r.   test_pin_joint_arbitrary_axisr   j  s    *%DAq  !JAq!QS!Q!##.SSD$)))CCKKN?acc!!!558vz !CF7SVG4 !CF7CF35 6 6 6 6 <<?ae###<<?$$&$q!t*444<<  .!333<<  .66q9BBDIII<<A!###  !JAq!QS!Q!##>33QSS!Q&&&33;;q>QSS   558vCF7SVG4( !CF7CF35 6 6 6 6 <<?ae###<<?""1%QSS000<<?$$&$q!t*444<<?  %***<<A!ACC%'''<<  .133$666LL!!!,,/77:CCEF133JQ#$ % $<<A,,QSS1RT999  !JAq!QS!Q!##ACC@33QSS!Q&&&33;;q>QSS   558vy!$QCF3 #A3q624 5 5 5 5 <<?ae###<<?""1%133...<<?$$&$q!t*444LLO))!##.E>>1a&!Q&&&<<A!###<<  .!##555  !JAq!QS!QQSSaccccACCi)acc''acc	23q888CC!##Iq!**,Q;;;AEE!H%	aT!WHSVOA%QxAq'89	aDGCFN1$d1gc!fnQ&67	
SVGSV1 *    <<?ae###LLO##A&//1qssUQqssU]DG#$ % $<<?$$&$q!t*444LLO))!##)4E>>1a&!Q&&&<<A'')a!##gtAw->>>><<  .!##);;;LL!!!,,/77:CCEa]ACC$q'#a&."2133"66GCFN1QSS !! " ! LLQ''*335!WHQJs1va#d1gaiA&6q&8&<<= > =<<A,,QSS1RT999!JAq!QS!QQSSaccccACCi!##o/acc''acc	ACC89Q>>>CC!##IO$$Q'002d1gacckAAAAEE!H%	aT!WHSRT]*1,	aQAX	q	 	"	aDGCBrE	N*1,	aQBY		!	#
q'!T!WSQrT"W--a/	aQ2b[!	!!	#	%1& *' ' ' ' <<?ae###<<?""1%..0QqssUQqssU]56qssU6;<@G5D D D D<<?$$&$q!t*444LLO))!##)ACC-8E>>1a&!Q&&&<<A'')aeaemT!W-DDDD<<  .!##);;;LL!!!,,/77:CCEa]ACC$q'#a"Q$h-"7"9!##"==GCBqDM!!#ACC'(( ) ( LLQ''*335GAIc!bd(m#A%acc)GAIc!bd(m#A%acc)** + * <<A,,QSS1RT999!JAq!Q5>DAqS!QQW!acc'ccACCi!##o ssQSSy1330 SS133Y_++ACC!##IO<Ee!!!SSWQSS[!!!$--/AYq[1Q46!133&!A$q&1SRT]?13D*Dacc)IIs1r!t8}_Q1a',-- . - AEE!H%	
1a!CF(1*	bQAX.q01Q469	
3q2a4x=	QqT!V	#	%	
3q2a4x=	QqT!V	#Qs1vXaZ!A$q&%8	
3q2a4x=	QqT!V	#	%	CBqDM	!	ad1f	$aA1Hoa&7!A$q&&@	
3q6!ad1f		1 *        <<?ququ}qu4d1g====<<?""1%..0QqssUQqssU]56qssU6;<@G5D D D D<<?$$&$q!t*444LLO))!##acc'!##+6E>>1a&!Q&&&LLQ((*GAIaKM!##Q	!Aacc 112 3 2<<  .!ACC%!ACC%-???LL!!!,,/77:CCEAc!fHqL!!##QSS(1aA1Ho/ ,,, !CBqDM/A-.q0455 6 5 LLQ''*335E!GCFN1QSS 1Q3q5QAX#6q#8#<<aCE#a"Q$h-!!##%&& ' & <<A""1339qss?3<<>!CCCr0   c                  <   t               \  } }}}t        j                  ||j                   |j                        }|j                  |j                  k(  sJ t        j                  ||j
                   |j
                        }|j                  |j                  k(  sJ t        j                  ||j                   |j                        }|j
                  |j
                  k(  sJ t        j                  ||j                   |j
                  z
  |j                  |j
                  z         }|j                  |j                  k(  sJ t        j                  ||j
                   |j                  z
  |j
                  |j                  z         }|j                  |j                  k(  sJ t        j                  ||j                   |j                  z
  |j                  |j                  z         }|j
                  |j
                  k(  sJ t        j                  ||j                   |j
                  z
  |j                  z
  |j                  |j
                  z   |j                  z         }|j
                  |j                  z
  |j
                  |j                  z
  k(  sJ y N)r/   r   _create_aligned_interframer)   r{   r*   )r   r    r!   r$   fs        r.   test_create_aligned_frame_pir     s   !JAq!Q((QSSD!##6A33!##::((QSSD!##6A33!##::((QSSD!##6A33!##::((QSSD133Jacc	BA33!##::((QSSD133Jacc	BA33!##::((QSSD133Jacc	BA33!##::((QSSD133J,<accACCi!##oNA339acc	!!!r0   c            
        	 t        d      \  	} t        d      \  }}}t        d	| ||      }|j                  |j                  k(  sJ t        dt        	      t        	      gdt        	       t        	      gg dg      }t        d      \  }}}t        d	| ||j                         j                  |      |k(  sJ t        d      \  }}}t        d	| |||j                          j                  |      |k(  sJ t        d      \  }}}t        t        	fd       y )	Nzq uTr6   )rp   rs   r   r   r   c                  <    t        d j                  z        S r   )r   r{   r   s   r.   r8   z%test_pin_joint_axis.<locals>.<lambda>  s    xQa!##gF r0   )r   r/   r   ro   r)   r	   r   r   r{   r   r   rT   )
r?   r    r,   r-   r6   N_R_Ar$   r   r!   rA   s
         @@@@r.   test_pin_joint_axisr     sE   % DAq+D1Aq!QdaAqD4PA<<466!!!QAA'QQ(  E ,D1Aq!QdS!Q1t558u+D1Aq!QdS!Q1t!558u+D1Aq!Qd
:FGr0   c                     t               \  } t        dj                  j                  z         }|j                  j
                  dk(  sJ |j                  j                  j                        j                  j                  z   k(  sJ |j                  j                  k(  sJ t               \  } j                  j                  dj                  j                  z         }t        d|j                        }|j                  |k(  sJ |j                  j                  k(  sJ t               \  } t        t        fd       t        d      t               \  } t        t        fd       t               \  } j                  j                  d| j                  z        t        t        fd	       t        d      t        t        fd
       y )Nr6   rm   	J_P_jointp)rm   rn   c                  T    t        d j                  j                              S Nr6   r   )r   r)   	to_matrix)r$   r   r!   s   r.   r8   z'test_locate_joint_pos.<locals>.<lambda>3  s    8CAACCMM!4DE r0   rA   c                  <    t        d j                  z        S r   r   r   s   r.   r8   z'test_locate_joint_pos.<locals>.<lambda>7  s    xQACCH r0   c                  "    t        d       S Nr6   )rn   r   r$   r!   rn   s   r.   r8   z'test_locate_joint_pos.<locals>.<lambda>:      xQ{K r0   c                  "    t        d       S r   r   r   s   r.   r8   z'test_locate_joint_pos.<locals>.<lambda>=  r   r0   )r/   r   r*   r{   rm   rt   rw   rx   rn   r~   r   r9   r   rT   r   )r    jointrm   r$   r   r!   rn   rA   s      @@@@@r.   test_locate_joint_posr   "  s   !JAq!QS!QQSS133Y7E""k111&&q||4acc	AAA,,,!JAq!Q<<))#qssQSSy9LS!Q\!"/E---,,,!JAq!Q
9EG 	sA!JAq!Q
:HI!JAq!Q,,((a!##g6K
:KL*K
:KLr0   c                  b   t               \  } }t        d      j                  | | j                  d       t	        d      }|j
                  k(  sJ |j
                  j                  |       dk(  sJ |j                  |k(  sJ t        d      }t               \  } }t        d      j                  | | j                  |       t        t        fd       t               \  } }t        d      j                  | | j                  d       t        t        fd       y )	N	int_framer<   r6   r   r   rA   c                  "    t        d       S r   r   )r$   r!   rp   s   r.   r8   z)test_locate_joint_frame.<locals>.<lambda>O  s    8CA9JK r0   c                  "    t        d       S r   r   )r$   r!   rs   s   r.   r8   z)test_locate_joint_frame.<locals>.<lambda>U  s    8CA8HI r0   )r/   r   r(   r{   r   rp   r   rs   r   r   rT   )r   r    r   rA   r$   r!   rs   rp   s       @@@@r.   test_locate_joint_framer   @  s   !JAq!Q&{3!!!QSS!,S!Q2CDE""&7777""--a0A555!!Q&&&sA!JAq!Q&{3!!!QSS!,
:KM  !JAq!Q%k2  ACC+
:IKr0   c            	      T   t               \  } } }}t        d      \  }}t        d||      }|j                  dk(  sJ |j                  |k(  sJ |j
                  |k(  sJ |j                  t        |g      k(  sJ |j                  t        |g      k(  sJ |j                  t        ||j                  t              z
  g      k(  sJ |j                  |j                  j                  k(  sJ |j                  j!                  |j"                        t%        d      k(  sJ |j&                  j!                  |j"                        t%        d      k(  sJ |j&                  j!                  |j                        | |j                  j                  z  k(  sJ |j"                  j!                  |j"                        | |j                  j                  z  k(  sJ |j"                  j)                  |j                        ||j                  j                  z  k(  sJ |j+                  |      dk(  sJ |j+                  |      dk(  sJ  |j,                         dk(  sJ t               \  }}}}t/        d      \  }}	t1        d      }
|
j3                  |j                  |j4                  t6        dz         t        d||||j                  j                  z  |	|j                  j4                  z  |j                  j8                  |
      }|j                  |j                  j8                  k(  sJ |j                  j!                  |j"                        |	|j                  j4                  z  k(  sJ |j&                  j!                  |j"                        ||j                  j                  z  k(  sJ |j&                  j!                  |j                        | |j                  j8                  z  k(  sJ |j"                  j!                  |j"                        | |j                  z  ||j8                  z  z
  |	|j4                  z  z   k(  sJ |j"                  j)                  |j                        ||j                  j8                  z  k(  sJ |j"                  j)                  |
      t%        d      k(  sJ |j+                  |      dk(  sJ |j+                  |      dk(  sJ t               \  } } }}t1        d      }
|
j3                  |j                  |j4                  t6        dz         t        d||||j                  j8                  z  |	|j                  j                  z  |j                  j8                  |
      }|j                  |j                  j8                  k(  sJ |j                  j!                  |j"                        |	|j                  j                  z  k(  sJ |j&                  j!                  |j"                        ||j                  j8                  z  k(  sJ |j&                  j!                  |j                        | |j                  j8                  z  k(  sJ |j"                  j!                  |j"                        | |z
  |j                  j8                  z  |	|j                  j                  z  z   k(  sJ |j"                  j)                  |j                        ||j                  j8                  z  k(  sJ |j+                  |      dk(  sJ |j+                  |      dk(  sJ y )	Nq_S, u_Sr   r   z&PrismaticJoint: S  parent: P  child: Crg   r%   r'   rl   )r/   r   r   rt   r2   r3   ru   r	   rS   rv   rU   rV   ro   r#   r)   rn   rw   rx   r   rm   r   r   rz   r   r   r(   r*   r   r{   )rd   r!   r$   rA   r?   r   r   r    r   r   r,   s              r.   test_sliding_jointr   X  s|   !JAq!Q*%DAqsAq!A66S==88q==77a<<==FA3K'''88vqc{"""66VQ]O,,,,<<17799$$$==!!!,,/6!9<<<>>""1<<0F1I===>>""1==1qS17799_DDD<<  .A#		/AAA<<AGG$AGGII555<<?a<<?a199;BBBB!JAq!Q5>DAq'"DQWWacc26*sAqq17799}#$qwwyy=QWWYY)-	/A <<17799$$$==!!!,,/1qwwyy=@@@>>""1<<0A		MAAA>>""1==1qS17799_DDD<<  .A#acc'AaccE/AaccE2IIII<<AGG$AGGII555<<D!VAY...<<?a<<?a!JAq!Q'"DQWWacc26*sAqq17799}#$qwwyy=QWWYY)-	/A <<17799$$$==!!!,,/1qwwyy=@@@>>""1<<0A		MAAA>>""1==1qS17799_DDD<<  .A26177992Dq{2RRRR<<AGG$AGGII555<<?a<<?ar0   c                     t        d      \  } }t               \  }}}}t        d|||j                          |j                   j	                  |j                        dk(  sJ |j                  j                  |       |j                  k(  sJ |j                  |      t        g dg dg dg      k(  sJ |j                  j                  |j                        | |j                  z  k(  sJ |j                  j                  |j                        j                  |      j                         |  |j                  z  k(  sJ |j                  j                  |      ||j                  z  k(  sJ |j                  j                  |      j                  |      | |j                  z  k(  sJ |j                  |      dk(  sJ |j                  |      dk(  sJ t               \  }}}}t        d|||j                  |j                         |j                  j	                  |j                        dk(  sJ |j                  j                  |      |j                  k(  sJ |j                  |      t        g dg d	g dg      k(  sJ |j                  j                  |      ||j                  z  k(  sJ |j                  j                  |      j                  |      ||j                  z  k(  sJ |j                  j                  |j                        | |j                  z  |j                  z
  k(  sJ |j                  j                  |j                        j                  |      j                         | |j                  z  |j                  z   k(  sJ |j                  |      dk(  sJ |j                  |      dk(  sJ t               \  }}}}t        d|||j                  |j                  
       |j                  j	                  |j                        dk(  sJ |j                  j                  |      |j                  k(  sJ |j                  |      t        g dg dg dg      k(  sJ |j                  j                  |      ||j                  z  k(  sJ |j                  j                  |      j                  |      ||j                  z  k(  sJ |j                  j                  |j                        |j                  | |j                  z  z   k(  sJ |j                  |      dk(  sJ |j                  |      dk(  sJ t               \  }}}}t        d|||j                  |j                  |j                  |j                  z          |j                  j	                  |j                  |j                  z         dk(  sJ |j                  |j                  z   j                  |      t        d      |j                  z  k(  sJ |j                  |      t        t        d      dz  t        d       dz  dgt        d      dz  t        d      dz  dgg dg      k(  sJ |j                  j                  |j                        | dz   |j                  z  |j                  z
  k(  sJ |j                  j                  |j                        j                  |      | t        d      dz  z
  dz   |j                  z  t        d      dz  |j                  z  z   k(  sJ |j                  j                  |      j                  |      ||j                  |j                  z   z  t        d      z  k(  sJ |j                  j                  |      ||j                  z  k(  sJ |j                  |      dk(  sJ |j                  |      dk(  sJ t               \  }}}}t        d|||j                  |j                  |j                  |j                  z   |j                  z
         |j                  j	                  |j                  |j                  z   |j                  z
        dk(  sJ |j                  |j                  z   |j                  z
  j                  |      t        d      |j                  z  k(  sJ t!        |j                  |            t        t        d      dz  t        d       dz  t        d      dz  gt        d      dz  t        d      dz  t#        d      dz  z   t#        d      dz  t        d      dz  z
  gt        d       dz  t#        d      dz  t        d      dz  z
  t        d      dz  t#        d      dz  z   gg      k(  sJ |j                  j                  |j                        | dz   |j                  z  |j                  z
  k(  sJ |j                  j                  |j                        j                  |      | t        d      dz  z
  dz   |j                  z  t        d      dz  |j                  z  z   t        d      dz  |j                  z  z
  k(  sJ |j                  j                  |      ||j                  z  k(  sJ |j                  j                  |      j                  |      t        d      |z  dz  |j                  z  t        d      |z  dz  |j                  z  z   t        d      |z  dz  |j                  z  z
  k(  sJ |j                  |      dk(  sJ |j                  |      dk(  sJ t               \  }}}}t%        d      \  }}t        d||||j                  z  ||j                  z  |j                  |j                  z   |j                  z
  |j                  |j                  z
  |j                  z          |j                  |j                  z
  |j                  z   j	                  |j                  |j                  z   |j                  z
        j                         dk(  sJ |j                  |j                  z   |j                  z
  j                  |      |j                  |j                  z
  |j                  z   k(  sJ t!        |j                  |            t        t#        d       dz  t#        d       dz  t#        d      dz  gt#        d      dz  t#        d      dz  t#        d      dz  gt#        d       dz  t#        d      dz  t#        d      dz  gg      k(  sJ |j                  j                  |j                        |t        d      | z  dz  z   |j                  z  t        d      | z  dz  |j                  z  z
  t        d      | z  dz  |j                  z  z   ||j                  z  z
  k(  sJ |j                  j                  |j                        j                  |      ||dz  z   t        d      | z  dz  z   |j                  z  d|z  dz  t        d      | z  dz  z
  |j                  z  z   d|z  dz  t        d      | z  dz  z   |j                  z  z   k(  sJ |j                  j                  |      t        d      |z  dz  |j                  z  t        d      |z  dz  |j                  z  z
  t        d      |z  dz  |j                  z  z   k(  sJ |j                  j                  |      j                  |      t        d      |z  dz  |j                  z  t        d      |z  dz  |j                  z  z   t        d      |z  dz  |j                  z  z
  k(  sJ |j                  |      dk(  sJ |j                  |      dk(  sJ y )Nr   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	   rx   rw   r   r   r   r*   r   r{   r
   r   r   )rA   r?   r   r    r!   r$   r   r   s           r.   !test_sliding_joint_arbitrary_axisr     s9   *%DAq!JAq!Q31t4SSD$)))CCKKN?acc!!!558vz:yABBBB<<  .!acc'999<<  .66q9BBDQSSPPP<<A!acc')))<<A&&q)aR!##X555<<?a<<?a  !JAq!Q31qssD33QSS!Q&&&33;;q>QSS   558vz9i@AAAA<<A!acc')))<<A&&q)QW444<<  .!ACC%!##+===<<  .66q9BBD!##PQPSPSSSS<<?a<<?a  !JAq!Q31!##F33QSS!Q&&&33;;q>acc!!!558vy*i@AAAA<<A!acc')))<<A&&q)QW444<<  .!##!##+===<<?a<<?a  !JAq!Q31133ACC$%CC!##I/33QSS133Y'1,,,CC!##Iq!T!WQSS[000558vQ	DG8A:q9DGAItAwqyRS;TV_`aaaa<<  .1q5!##+2CCCC<<  .66q9a$q'!)ma>OQRQTQT=TW[\]W^_`W`abadadWd=dddd<<A&&q)Q!##)_T!W-DDDD<<A!ACC%'''<<?a<<?a!JAq!Q31133ACC$%CC!##IO533QSS133Y_-222CC!##IO$$Q'47133;666AEE!H%$q'!)d1gXaZaQR1S26q'!)T!WQY1a=OQRSTQUVWQWZ^_`ZabcZcQc1d377(1*ad1ftAwqy>PRVWXRYZ[R[^_`a^bcd^dRd1e1g *h h h h <<  .1q5!##+2CCCC<<  .66q9	
T!WQY	ACC$q'!)ACC-/$q'!)ACC-?@ @ @<<A!ACC%'''<<A&&q)T!WQYq[_tAwqy{133-NQUVWQXYZQZ[\Q\]^]`]`Q`-````<<?a<<?a!JAq!Q5>DAq311QSS5ae$%CC!##IO%&SS133Y_6 CCGACCK&&qss133wqss{3<<>!CCCCCGACCK  #qssQSSy133666AEE!H%1Q4%'AaD57AaDF1K23A$q&!A$q&!A$q&1I34Q4%'1Q461Q461J1L *M M M M <<  .	
T!WQYq[!##Q	!ACC/$q'!)A+acc/AAaccEIJ J J<<  .66q9	
QqS4719Q;	#qs1utAwqy{':ACC&??2a46DQRGTUIVWKCWYZY\Y\B\\] ] ]<<A$q'!)A+acc/DGAIaKO"Cd1gaiPQkRSRURUo"UUUU<<A&&q)T!WQYq[_tAwqy{133-NQUVWQXYZQZ[\Q\]^]`]`Q`-````<<?a<<?ar0   c                  
   t               \  } }}}t        d      \  }}}}t        d||      }|j                  dk(  sJ |j                  |k(  sJ |j
                  |k(  sJ |j                  t        ||g      k(  sJ |j                  t        ||g      k(  sJ |j                  |k(  sJ |j                  |k(  sJ |j                  |k(  sJ |j                  |k(  sJ |j                  t        ||j                  t              z
  ||j                  t              z
  g      k(  sJ |j                   | j"                  k(  sJ |j$                  j'                  |j(                        t+        d      k(  sJ |j,                  j'                  |j(                        t+        d      k(  sJ |j,                  j'                  |j.                        | | j"                  z  k(  sJ |j(                  j'                  |j(                        || j"                  z  k(  sJ |j$                  j1                  |       || j"                  z  k(  sJ |j3                  |       || j"                  z  k(  sJ |j4                  | k(  sJ |j6                  |k(  sJ |j9                         dk(  sJ t        d      \  }	}
}}t;        d      \  }}t        d      \  } }}}}}t        d|||	|||| j"                  z  ||j<                  z  ||d| j>                  z  	      }|j                  t        |	|g      k(  sJ |j                  t        ||g      k(  sJ |j                  |	k(  sJ |j                  |k(  sJ |j                  |k(  sJ |j                  |k(  sJ |j                  t        ||	j                  t              z
  ||j                  t              z
  g      k(  sJ |j                   d| j>                  z  k(  sJ |j$                  j'                  |j(                        ||j<                  z  k(  sJ |j,                  j'                  |j(                        || j"                  z  k(  sJ |j,                  j'                  |j.                        | | j>                  z  k(  sJ |j(                  j'                  |j(                        || j"                  z  || j>                  z  z   ||j<                  z  z
  k(  sJ |j(                  j1                  |       || j>                  z  ||z  |j>                  z  z
  k(  sJ |j3                  |       || j>                  z  k(  sJ y )
Nzq0:2_J, u0:2_Jr6   r   z(CylindricalJoint: J  parent: P  child: Cz
q0:2, u0:2l, mTr'   )rotation_coordinaterotation_speedtranslation_speedrm   rn   rp   rs   ro   ) r/   r   r   rt   r2   r3   ru   r	   rS   r  translation_coordinater  r  rv   rU   rV   ro   r)   rn   rw   rx   r   rm   ry   r   r   rp   rs   rz   r   r*   r{   )r   r    r!   r$   q0_defq1_defu0_defu1_defCjrF   rG   rJ   rK   r   r   r,   r-   s                    r.   test_cylindrical_jointr    s   !JAq!Q%34D%E"FFFF	#q!	$B77c>>99>>88q==>>VVV$4555599/0000!!V+++$$...&&&6)))77ffv{{1~5vA7NOPPPP==ACC>>""1<<0F1I===??##ALL1VAY>>>??##BOO4!##EEE<<  .&133,>>>>>a FQSSL000<<?fqssl***1$$$!###::<EEEE#L1NBB6?DAq+D1Aq!Qd	#q!B,.QW&'!##g+/AG
EB >>VRL111199Bx((((!!R'''$$..."""2%%%77fb2771:orFKKN/BCDDDD==AG###>>""1<<0AG;;;??##ALL1QW<<<??##BOO4!##EEE<<  	QSS6ACC</!acc'9: : :<<A"qss(R!Vacc\"9999<<?b133h&&&r0   c                  ~   t               \  } }}}t        d      \  }}}t        d      \  }}}	t        d||      }
|
j                  dk(  sJ |
j                  |k(  sJ |
j
                  |k(  sJ |
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        ||j                  t              z
  ||j                  t              z
  |	|j                  t              z
  g      k(  sJ |
j                   | j"                  k(  sJ |
j$                  | j&                  | j(                  gk(  sJ |
j*                  j-                  |j.                        t1        d      k(  sJ |
j2                  j-                  |j.                        t1        d      k(  sJ || j&                  z  || j(                  z  z   }|
j2                  j-                  |
j*                        | k(  sJ |j.                  j-                  |j.                        |k(  sJ |
j*                  j5                  |       || j&                  z  |	| j(                  z  z   k(  sJ |j7                  |       || j"                  z  k(  sJ |
j8                  | k(  sJ |
j:                  |k(  sJ |
j=                         dk(  sJ t        d      \  }}}}}}t?        d      \  }}t        d      \  } }}}}}t        d|||||g||g|| j"                  z  ||j&                  z  ||	
      }
|
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        ||j                  t              z
  ||j                  t              z
  ||j                  t              z
  g      k(  sJ |
j                   |j"                  k(  sJ |
j$                  |j&                  |j(                  gk(  sJ |
j*                  j-                  |j.                        ||j&                  z  k(  sJ |
j2                  j-                  |j.                        || j"                  z  k(  sJ |
j2                  j-                  |
j*                        || j&                  z  || j(                  z  z   k(  sJ |j.                  j-                  |j.                        || j"                  z  || j&                  z  z
  || j(                  z  z
  ||j&                  z  z
  k(  sJ |j.                  j5                  |       | | j&                  z  || j(                  z  z
  ||z  |j"                  z  z   k(  sJ |j7                  |       || j"                  z  k(  sJ y )
Nzq0:3_Jzu0:3_Jr6   r   z#PlanarJoint: J  parent: P  child: C
q0:3, u0:3r  T)r  planar_coordinatesplanar_speedsrm   rn   rp   rs   ) r/   r   r   rt   r2   r3   ru   r	   rS   r  r  r  r  rv   rU   rV   rotation_axisr)   planar_vectorsr*   r{   rn   rw   rx   r   rm   r   r   rp   rs   rz   r   )r   r    r!   r$   r  r  q2_defr  r	  u2_defr
  r_P_CrF   rG   r`   rJ   rK   rb   r   r   r,   r-   s                         r.   test_planar_jointr    s   !JAq!Q+H5FFF+H5FFF	S!Q	B77c>>99>>88q==>>VVVV$<====9978888!!V+++  FFF+;$<<<<&&&vvv&6777777ffv{{1~5vA7N$v{{1~57 8 8 8 8qss"""acc
***>>""1<<0F1I===??##ALL1VAY>>>QSSL6ACC<'E??##BNN3v===<<  .%777>>a FQSSL6ACC<$????<<?fqssl***1$$$!###::<@@@@+L9BBB6?DAq+D1Aq!Qd	S!QB)+RR"#acc'q133w'+d
DB >>VRRL111199B/0000!!R'''  FB8$4444&&&vr2h////77ffrwwqz12
? 2771:o/ 0 0 0 0tvv%%% 0000>>""1<<0AG;;;??##ALL1QW<<<??##BNN3rACCx"qss(7JJJJ<<  	QSS28+b133h6QSS@A A A<<A2#)b133h"6!acc9I"IIII<<?fqssl***r0   c                  >   t        d      \  } }}}}}t        d      \  }}t               \  }}	}
}t        d|
|| ||g|||g||j                  z  | |j                  z  |j                  |j
                  t        d      z  z   |	j                  |	j
                  t        d      z  z         }|j                  j                  |      |j                  |j
                  t        d      z  z   j                         k(  sJ |j                  j                  |	      |	j                  |	j
                  t        d      z  z   j                         k(  sJ |j                  j                  |j                        t        dz  k(  sJ |j                  |	      j                  | d|d|di      t        d      k(  sJ t        t!        |       t        d       t#        |       z  dz  t#        |       dz  gt        d      t#        |       z  dz  dt!        |       z  d	z  d
z   t        d      dt!        |       z
  z  d	z  gt#        |        dz  t        d      dt!        |       z
  z  d	z  t!        |       d	z  dz   gg      }t%        |j                  |	      |z
        t'        d      k(  sJ y )Nr  zl1:3r6   r>   )rm   rn   rp   rs   r   r   r'   r   g      ?r<   g      ?)r   r   r/   r   r{   r*   r   r  r   	normalizer   r   r   r   r   r	   r   r   r
   r   )rF   rG   r`   rJ   rK   rb   l1l2r   r    r!   r$   r6   r   s                 r.   test_planar_joint_advancedr  U  sM    ,L9BBBV_FB!JAq!QCArB8R"b!#acc!#acc	&'ccACC$q'M&9%&SS133a=%8		:A
 ??""1%!##d1g*=)H)H)JJJJ??""1%!##d1g*=)H)H)JJJJ??((-a777558b!RB23s1v===	R47(SW$q(#b'A+6	a3r7	Q	CGa% 7	aABK	 1	$	&
b'AtAw!c"g+.2CGaK%4GH	J KE AEE!Hu,-q999r0   c            	         t               \  } }}}t        d      \  }}}}}}	t        d||      }
|
j                  dk(  sJ |
j                  |k(  sJ |
j
                  |k(  sJ |
j                  t        |||g      k(  sJ |
j                  t        |||	g      k(  sJ |
j                  t        ||j                  t              z
  ||j                  t              z
  |	|j                  t              z
  g      k(  sJ |
j                  j                  |j                        t        d      k(  sJ |
j                   j                  |j                        t        d      k(  sJ |
j                   j                  |
j                        t        d      k(  sJ |j                  j                  |j                        t        d      k(  sJ |j                  j#                  |       t        d      k(  sJ |j%                  |      | t'        |      z  t'        |      z  |t)        |      z  z
  |j*                  z  |t)        |      z  t'        |      z  |t'        |      z  z
  |j,                  z  z   | t)        |      z  |	z
  |j.                  z  z   k(  sJ |j%                  |      |t'        |      z  t'        |      z  |t)        |      z  z   |j*                  z  | t)        |      z  t'        |      z  |t'        |      z  z   |j,                  z  z   |t)        |      z  |	z   |j.                  z  z   k(  sJ  |
j0                         dk(  sJ |
j2                  dk(  sJ |
j4                  dk(  sJ |
j6                  J y )Nq0:3_S, u0:3_Sr   r   z&SphericalJoint: S  parent: P  child: CBODY{   )r/   r   r   rt   r2   r3   ru   r	   rS   rv   rU   rV   rn   rw   rx   r   rm   r   r   r   r   r)   r*   r{   rz   	_rot_type
_rot_order_amounts)r   r    r!   r$   rF   rG   r`   rJ   rK   rb   r   s              r.   test_spherical_jointr"  p  s   !JAq!Q+,<=BBBsAq!A66S==88q==77a<<==FBB<000088vr2rl++++66VR"''!*_b2771:orBGGAJOPPPP==!!!,,/6!9<<<>>""1<<0F1I===>>""1==1VAY>>><<  .&);;;<<A&)+++<<?sSW}s2w6c"gELRL3r7"R#b'\1QSSP9 93R=2%<-- - - - <<?rCG|c"g5SWDKC#b'MCG#b3r7l2accO: :B<"$=,, , , , 199;BBBB;;&   <<3::r0   c            	      |   t        d      \  } }}t        dd      \  }}}t               \  }}}}	t        d||	| ||g|||g      }
|
j                  t	        | ||g      k(  sJ |
j
                  t	        |||g      k(  sJ |
j                  t	        g d      k(  sJ |j                  |	      | t        |      z  t        |      z  |t        |      z  z
  |j                  z  |t        |      z  t        |      z  |t        |      z  z
  |j                  z  z   | t        |      z  |z
  |j                  z  z   k(  sJ y )Nzq0:3r<   r   )ru   rS   )r   r   r   )r   r/   r   ru   r	   rS   rv   r   r   r   r)   r*   r{   )rF   rG   r`   rJ   rK   rb   r   r    r!   r$   r   s              r.   /test_spherical_joint_speeds_as_derivative_termsr$    s>     'JBB*JBB!JAq!QsAqr2rlBB<PA==FBB<000088vr2rl++++66VI&&&&<<?sSW}s2w6c"gEL
SWs2wc"g-P5 5sSW}r!QSS8)) ) ) )r0   c                    	
 t        d      \  } }}}}}t        d      \  t               \  }}
	t        d
	gg      }|j                  t	        g      k(  sJ |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       t        t        	
f	d       t        t        	
f	d       y )Nr  z
q0:4, u0:4r   c            	      L    t        d t        g      t        g            S Nr   r   r	   )r$   r!   rF   rG   rJ   s   r.   r8   z-test_spherical_joint_coords.<locals>.<lambda>  s#    >#q!VRH-=vrd|L r0   c                  T   	 t        d t        g      t        g            S r'  r(  )	r$   r!   rF   rG   r`   ra   rJ   rK   rb   s	   r.   r8   z-test_spherical_joint_coords.<locals>.<lambda>  s1    ~Q62r2r*+VRRL-A C r0   c                  T   	 t        d t        g      t        g            S r'  r(  )	r$   r!   rF   rG   r`   rJ   rK   rb   u4s	   r.   r8   z-test_spherical_joint_coords.<locals>.<lambda>  s1    ~Q62r2,'RR0@)A C r0   )r   r/   r   ru   r	   rS   r   rT   )q0sq1sq2su0su1su2sr   r    r   r$   r!   rF   rG   r`   ra   rJ   rK   rb   r+  s            @@@@@@@@@@r.   test_spherical_joint_coordsr2    sb   #12B#C Cc3S%3L%A"BBBB!JAq!QsAq2r2,R=A==FBB<000088vr2rl++++!JAq!QsAq&"b""6r2rl+	-A==FBB<000088vr2rl++++!JAq!Q
:LN : C C D
: C C Dr0   c                     t        d      \  t        t               t               t              z  t              t              z  gt               t              z  t              t              z  t              z  t              t              z  z
  t               t              z  t              z  t              t              z  z
  gt              t              z  t               t              z  t              t              z  t              z  z
  t               t              z  t              t              z  t              z  z   gg      } t         t              z  z
  g t              z  t              z  t              z  z   gt              z  t              z  t              z  z   gg      }t        t	        d       t        t
        dz  z         z  t              z  t        t
        dz  z         z  z   z  g t              z  z
  g t              z  z
  gg      }t        d      \  }}}}}}t        d||gg|j                  |j                  z   |j                   |j                  z   ||dd      }	|	j                  j                         d	k(  sJ |	j                  dk(  sJ t        |j                  |      | z
        t!        d
      k(  sJ |j#                  |      j%                  |      |k(  sJ |j&                  j)                  |      j%                  |      |k(  sJ t        d      \  }}}}}}t        d||gg|j                  |j                  z   |j                   |j                  z   ||d	fd      }	fd}
|	j                  j                         d	k(  sJ |	j                  dk(  sJ t        |j                  |       |
|       z
        t!        d
      k(  sJ |j#                  |      j%                  |       |
|      k(  sJ |j&                  j)                  |      j%                  |       |
|      k(  sJ t        d      \  }}}}}}t        d||gg|j                  |j                  z   |j                   |j                  z   ||dd      }	|	j                  j                         d	k(  sJ |	j                  dk(  sJ t        |j                  |      t        t               t              z  t              t              z  t              z  t              t              z  z
  t              t              z  t              z  t              t              z  z   gt               t               t              z  t               t              z  gt              t              z  t               t              z  t              t              z  t              z  z
  t              t              z  t              t              z  t              z  z
  gg      z
        t!        d
      k(  sJ |j#                  |      j%                  |      t        t              z  z
  gt              z  t              z  t              z  z
  gt              z  t              z  t              z  z   gg      k(  sJ |j&                  j)                  |      j%                  |      t        t	        d       t        t
        dz  z         z  t              z  t        t
        dz  z         z  z   z  gt              z  z
  gt              z  z
  gg      k(  sJ y )Nr  r'   r   Tr   bodyr  ru   rS   rm   rn   rp   rs   rot_type	rot_orderr  r>   	ru   rS   rm   rn   rp   rs   r6  amountsr7  c                 >    | j                  i      S r   r   exprrF   rG   r`   rJ   rK   rb   s    r.   r8   z2test_spherical_joint_orient_body.<locals>.<lambda>  ,    	RRRRRR8!: r0   BodYyxz)r   r	   r   r   r   r   r/   r   r)   r*   r{   r  upperr   r
   r   r   r   r   rx   r   r   N_w_AN_v_Cor   r    r!   r$   r,   r-   r   switch_orderrF   rG   r`   rJ   rK   rb   s              @@@@@@r.    test_spherical_joint_orient_bodyrF    s=   +L9BBB
b'CG8c"g%s2wR'89
b'CG	SWs2w.R83r7SW;LL
b'CG	c"g	%B#b'(9	9	;	R3r7	SWHs2w.R3r71BSW1LL
b'CG	c"gB/#b'9	9	;	< =E bS3r7]R'(S3r7]SW,rCG|;<#b'\CG+b3r7l:;= >E 
q'R#b26k**SW4rCR!V<L7LL	MN
s2w	sSW}r124 5F ,D1Aq!QdsAqr2rlBB<$%CC!##IACC4!##:)- &#	7A ;;&(((<<3AEE!Hu,-q999<<?$$Q'5000<<A((+v555+D1Aq!QdsAqr2rlBB<$%CC!##IACC4!##:)- &R	MA: :L;;&(((<<3AEE!H|E'::;uQxGGG<<?$$Q'<+>>>><<A((+|F/CCCC+D1Aq!QdsAqr2rlBB<$%CC!##IACC4!##:)- &%	9A ;;&(((<<5   AEE!Hv
b'CG	SWs2w.R83r7SW;LL	R3r7	SW	$s2wR'8	8	:
b'CG8c"g%Bx#b''9:	R3r7	SWHs2w.R3r71BSW1LL	R3r7	SWs2w.R8	8	:	/; (< < =
 AFaI I I <<?$$Q'6	c"g	b3r7lSW4rCG|CD	c"gB	"s2w,	./31 ,2 2 2 2 <<A((+v
q'R#b26k**SW4rCR!V<L7LL	MN	c"g	b3r7lR/072 03 3 3 3r0   c                     t        d      \  t        t               t              z  t              t              z  t              z  z
  t              t              z  t              z  t              t              z  z
  t              t              z  gt               t              z  t              t              z  t              z  z   t               t              z  t              z  t              t              z  z
  t               t              z  gt              t              z  t               t              z  t              gg      } t        t              z  t              z  t              z  z
  gt              z  t              z  t              z  z   gt              z  z
  gg      }t        t              z  z
  gt              z  z
  gt	        d       t        t
        dz  z         z  t        t
        dz  z         z  t              z  z   z  gg      }t        d      \  }}}}}}t        d||gg|j                  |j                  z   |j                   |j                  z   ||dd      }	|	j                  j                         d	k(  sJ |	j                  dk(  sJ t        |j                  |      | z
        t!        d
      k(  sJ t        |j#                  |      j%                  |            |k(  sJ t        |j&                  j)                  |      j%                  |            |k(  sJ fd}
t        d      \  }}}}}}t        d||gg|j                  |j                  z   |j                   |j                  z   ||d	fd      }	|	j                  j                         d	k(  sJ |	j                  dk(  sJ t        |j                  |       |
|       z
        t!        d
      k(  sJ t        |j#                  |      j%                  |             |
|      k(  sJ t        |j&                  j)                  |      j%                  |             |
|      k(  sJ t        d      \  }}}}}}t        d||gg|j                  |j                  z   |j                   |j                  z   ||dd      }	|	j                  j                         d	k(  sJ |	j                  dk(  sJ t        |j                  |      t        t               t              z  t               t              z  t              t              z  t              z  z   t              t              z  t              z  t              t              z  z   gt               t               t              z  t               t              z  gt              t              z  t               t              z  t              t              z  t              z  z
  t               t              z  t              z  t              t              z  z   gg      z
        sJ t        |j#                  |      j%                  |      t         t              z  z   g t              z  t              z  t              z  z   gt              z  t              z  t              z  z   gg      z
        t!        d
d      k(  sJ t        |j&                  j)                  |      j%                  |      t        t              z  t              z  t              z  z   gt              z  t              z  t              z  z   gt              z  z   t              z  z
  t              z  t              z  z
  gg      z
        t!        d
d      k(  sJ y )Nr  r'   r   Tr   spacer  r5  SPACEr>   c                 >    | j                  i      S r   r;  r<  s    r.   r8   z3test_spherical_joint_orient_space.<locals>.<lambda>	  r>  r0   r8  SPaCezxyr<   )r   r	   r   r   r   r   r/   r   r)   r{   r*   r  rA  r   r
   r   r   r   r   rx   r   rB  s              @@@@@@r.   !test_spherical_joint_orient_spacerM    s   +L9BBB
b'CG	c"gB/#b'9	9	R3r7	SW	$s2wR'8	8#b'CG:K	M
b'CG	c"gB/#b'9	9
b'CG	c"g	%B#b'(9	9CG8c"g;M	O	R3r7	SWHs2w.B8: ;E 	c"gSWs2w.	./	c"gSWs2w.	./"rCG|2C1DF GE 	b3r7l	b2B</0	aRC#b26k**R#b26k2B-BSW-LL	MNP QF ,D1Aq!QdsAqr2rlBB<$%CC!##IACC4!##:)- '3	8A ;;')))<<3AEE!Hu,-q999ALLO55a89UBBBALL,,Q/99!<=GGG: :L+D1Aq!QdsAqr2rlBB<$%CC!##IACC4!##:)- '"b"	NA ;;')))<<3AEE!H|E'::;uQxGGGALLO55a89\%=PPPPALL,,Q/99!<=fAUUUU+D1Aq!QdsAqr2rlBB<$%CC!##IACC4!##:)- '5	:A ;;')))<<5   AEE!Hv
b'CG	c"gXB/#b'CG2Cc"g2MM	R3r7	SW	$s2wR'8	8	:
b'CG8c"g%Bx#b''9:	R3r7	SWHs2w.R3r71BSW1LL
b'CG	c"g	%B#b'(9	9	;	/< (= = > > > ALLO55a86
rCG|	sSW}rCG|c"g/EEF	c"gSWs2w.	./C1 <2 2 36;AqkB B B ALL,,Q/99!<v	c"gSWs2w.	./	c"gSWs2w.	./	b3r7l	R#b'\	)	c"gB	
  	!G" @# # $ (-Q{	3 3 3r0   c                     t               \  } } }}t        d||      }|j                  dk(  sJ |j                  |k(  sJ |j                  |k(  sJ |j
                  t               k(  sJ |j                  t               k(  sJ |j                  t        ddg       j                  k(  sJ |j                  |      t        d      k(  sJ |j                  j                  |j                        t        d      k(  sJ |j                   j                  |j                        t        d      k(  sJ |j                   j                  |j                        t        d      k(  sJ |j                  j                  |j                        t        d      k(  sJ |j                  j#                  |j$                        t        d      k(  sJ |j'                  |      dk(  sJ |j'                  |      dk(  sJ |j)                         dk(  sJ t               \  }}}}t+        d      \  }}t-        d      }|j/                  |j$                  |j0                  t2        dz         t        d||||j$                  j4                  z  ||j$                  j0                  z  |	      }|j                  j                  |j                        ||j$                  j0                  z  k(  sJ |j                   j                  |j                        ||j$                  j4                  z  k(  sJ |j                   j                  |j                        t        d      k(  sJ |j                  j                  |j                        | |j4                  z  ||j0                  z  z   k(  sJ |j                  j#                  |j$                        t        d      k(  sJ |j                  j#                  |      t        d      k(  sJ |j'                  |      dk(  sJ |j'                  |      dk(  sJ |j4                  |j6                  k(  sJ t9        ||       y )
NWr<   r   r>   z!WeldJoint: W  parent: P  child: Crg   r%   r'   )rm   rn   rp   )r/   r   rt   r2   r3   ru   r	   rS   rv   Tr   r   rn   rw   rx   r   rm   r   r#   r   rz   r   r   r(   r*   r   r)   r{   r   )	rd   r!   r$   rO  r   r    r   r   r,   s	            r.   test_weld_jointrQ  -  sk   !JAq!Q#q!A66S==88q==77a<<==FH$$$88vx66VAq"%'''''558s1v==!!!,,/6!9<<<>>""1<<0F1I===>>""1==1VAY>>><<  .&);;;<<AGG$q	111<<?a<<?a99;====!JAq!Q5>DAq'"DQWWacc26*#q!!aggii-!''))mt	EA ==!!!,,/1qwwyy=@@@>>""1<<0A		MAAA>>""1==1VAY>>><<  .A#)a!##g2EEEE<<AGG$q	111<<D!VAY...<<?a<<?a33!##::Ar0   c            
      8   t        d      \  } }t               \  }}}}t               5  t        d|||j                          d d d        |j                   j                  |j                        dk(  sJ |j                  j                  |       |j                  k(  sJ |j                  |      t        g ddt        |        t        |        gdt        |        t        |       gg      k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      j                         t        |dz        k(  sJ t               \  }}}}t               5  t        d|||j                  |j                  z          d d d        |j                  j                  |j                  |j                  z         dk(  sJ |j                  j                  |      |j                  |j                  z   t        d      z  k(  sJ |j                  |      t        t        d      dz  t        d      dz  dgt        d       dz  t        d      dz  dgg dg      k(  sJ |j                  |      t!        d      k(  sJ y # 1 sw Y   AxY w# 1 sw Y   xY w)	Nrh   r6   )
child_axisr   r   r'   )parent_axisr   )r   r/   r   r   r)   r   r   r   r	   r   r   r   r   r   r   r*   r   )rA   r?   r   r    r!   r$   s         r.   !test_deprecated_parent_child_axisrU  T  sD   *%DAq!JAq!Q		! -at,-SSD$)))CCKKN?acc!!!558vz !CF7SVG4 !CF7CF35 6 6 6 6 <<?a!##g%%%<<?$$&$qAv,666!JAq!Q		! 9sAqaccACCi89CCqssQSSy)Q...33;;q>accACCi472222558vQ!T!Wq[!<!%a1d1gk1=yJ K K K K<<?fQi'''#- -9 9s   J'JJJc            	         t               \  } }}}t               5  t        d||| j                  | j                  z   |j                  |j
                  z
        }d d d        j                  j                  |j                        | j                  | j                  z   k(  sJ |j                  j                  |j                        |j                  |j
                  z
  k(  sJ t               \  } }}}t               5  t        d||| j
                  | j                  z   |j                  |j                  z
        }d d d        j                  j                  |j                        | j
                  | j                  z   k(  sJ |j                  j                  |j                        |j                  |j                  z
  k(  sJ y # 1 sw Y   oxY w# 1 sw Y   xY w)Nr6   )parent_joint_poschild_joint_pos)r/   r   r   r)   r*   r{   rm   rw   rx   rn   r   )r   r    r!   r$   r   sliders         r.   test_deprecated_joint_posrZ  k  sp   !JAq!Q		! 2sAq1339'(ssQSSy22 $$Q\\2accACCi?????##ALL1QSS133Y>>>!JAq!Q		! ;QACC!##I01acc	;; ''5qssBBB&&q||4acc	AAA2 2; ;s   ?G<?GGGN)F)Bsympy.core.functionr   sympy.core.numbersr   sympy.core.singletonr   (sympy.functions.elementary.miscellaneousr   (sympy.functions.elementary.trigonometricr   r   sympy.core.backendr	   r
   r   r   sympy.core.symbolr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   sympy.physics.mechanics.jointr   sympy.physics.vectorr   r   r   sympy.testing.pytestr   r   simp_trV   r/   r:   re   r   r   r   r   r   r   r   r   r   r   r   r   r  r  r  r"  r$  r2  rF  rM  rQ  rU  rZ   r0   r.   <module>ri     s    * ! " 9 ? C C %M M M 0 > > ? 
9,O^6&r%AP,?^I/X5"pLD^"$H.M<K03 l\ ~.'b4+n:66)D293x;3|$N(.Br0   