
    Rie+                        d dl mZ d dlmZ d dlmZ d dlmZ d dlm	Z	m
Z
 d dlmZmZmZmZ d dlmZ d dlmZmZmZmZmZmZmZmZmZ d d	lmZ d d
lmZm Z m!Z! d dl"m#Z#m$Z$ de_%        ej&        Z'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' ZAd(S )*    )
expand_mul)pi)S)sqrt)cossin)Matrix_simplify_matrixeyezeros)symbols)	dynamicsymbolsBodyJointsMethodPinJointPrismaticJointCylindricalJointPlanarJointSphericalJoint	WeldJointJoint)VectorReferenceFramePoint)raiseswarns_deprecated_sympyTFc                 j   t          d          }t          d          }t          d|          }t          d|          }| rlt          d          t          d          }}|                    ||j        t                     |                    ||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          Hlib/python3.11/site-packages/sympy/physics/mechanics/tests/test_joint.py_generate_bodyr/      s    sAsASASA &#G,,nW.E.EdAC$$$AC"q)))!Q4%%aA:    c                  x     t          d          t          d           t          t           fd           d S )Nparentchildc                  &    t          d           S NJr   r3   r2   s   r.   <lambda>ztest_Joint.<locals>.<lambda>$   s    eC77 r0   )r   r   	TypeErrorr7   s   @@r.   
test_Jointr:   !   s=    (^^FMME
97777788888r0   c            	         t          d          \  } }}}t          d          \  }}}}}}	}
t          d          \  }}}}t                      \  }}t          d                              | d          t	          | g          k    sJ                     |gd          t	          |g          k    sJ                     |gdd          t	          |g          k    sJ                     d d          t	          |g          k    sJ                     d gd          t	          |g          k    sJ                     d d gd          t	          |g          k    sJ                     d d          t	          ||g          k    sJ                     g d          t	          ||g          k    sJ                     g dd          t	          ||g          k    sJ                     d |gdd          t	          ||g          k    sJ                     d dd          t	          ||g          k    sJ                     d dd	          t	          |g          k    sJ                     g dd	          t	          |||	g          k    sJ                     d dd
          t	          |g          k    sJ                     g dd	dd
          t	          |	g          k    sJ                     g dd          t	          ||g          k    sJ t          t          fd           t          t          fd           t          t          fd           t          t          fd           t          t          fd           t                      \  }}t          d                    t                               t                      \  }}t          t          fd           t          t          fd           d S )Nzq u q_J u_Jzq0:4_J u0:4_Jz	q0:4 u0:4r6      r'   )offset   uT)number_singleqc                  4                          gd          S )Nr<   _fill_coordinate_listr6   q0q1s   r.   r8   z,test_coordinate_generation.<locals>.<lambda>G       q66BxCC r0   c                  8                          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N r0   c                  4                          gd          S )Nr>   rC   rE   s   r.   r8   z,test_coordinate_generation.<locals>.<lambda>I   rH   r0   c                  N                          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M r0   c                  :                          z   gd          S )Nr'   rC   rE   s   r.   r8   z,test_coordinate_generation.<locals>.<lambda>L   s    a55rBwmQG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K r0   c                  0    t          d g          S )Nr6   )speedsrP   )r$   r!   rJ   rK   s   r.   r8   z,test_coordinate_generation.<locals>.<lambda>S   s    ~c1aRMM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   '   s   !-00LAq"b-;O-L-L*Cc3S#s%3K%@%@"BBBB!!JAq!QaA""1a((FA3KK7777""A3**faSkk9999""A3!"44sCCCC""4++vrd||;;;;""D61--====""Bd#3Q776	S#< <     ""4++vsCo/F/FFFFF""2q))VS#sO-D-DDDDD""2q"33vsCo7N7NNNNN""Bb>1Q"??6	S"D D    ""41"55c
9K9KKKKK""4C00FB4LL@@@@""2q#..&#sC2I2IIIII""4$"??63%==PPPP""2q#q$7763%==HHHH""2q#..&#sC2I2IIIII
:CCCCCCDDD
:NNNNNNOOO
:CCCCCCDDD
9MMMMMNNN
9GGGGGGHHH!!JAq!QS!QBGGAJJ'''!!JAq!Q
:KKKKKKLLL
:MMMMMMMNNNN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          ||                    t                    z
  g          k    sJ |j        | j        j        k    sJ |j                            |j                  t'          d          k    sJ |j                            | j                  t'          d          k    sJ |j                            |j                  t'          d          k    sJ |j                            | j                  t'          d          k    sJ |j        | j        k    sJ |j        |j        k    sJ |                                dk    sJ t          d          }t          d	          }t3          d
          }	|	                    |j        |j        t8          dz             t          d||||j        j        z  ||j        j        z  |j        j        |	          }
|
j        |j        j        k    sJ |
j                            |j                  ||j        j        z  k    sJ |
j                            |j                  ||j        j        z  k    sJ |
j                            |
j                  t'          d          k    sJ |j                            |j                  | |j        j        z  ||j        j        z  z   k    sJ |
j        |	k    sJ |
j        |j        k    sJ t          d          \  }}tA          d          \  }}} }}	}| j        !                    d|j        |j        z             }|j        !                    d|j        |j        z             }t          d| ||||||	||j        
  
        }|j        |j        k    sJ |j        "                    |          dk    sJ |j        |k    sJ |j        |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
  |j        z
  k    sJ |j        "                    |          #                    |          |tI          |          z  |tK          |          z  z
  |j        z  | tI          |          z  |tK          |          z  z
  |j        z  z   k    sJ |j        |	k    sJ |j        |k    sJ d S )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   s   S		AS		A5>>DAq*%%DAq	#q!		B7c>>>>9>>>>8q====>VQC[[((((9s####7fa!&&))m_------=AGI%%%%>""1<00F1II====?##AL11VAYY>>>>?##BO44q		AAAA<  ..&));;;;17****!'))))::<<=====	dB	dB'""DRXrtR!V,,,	$BQrxz\
lrxz$(
* 
* 
*B >RXZ''''?##BM22a"(*nDDDD$$R]33q28:~EEEE$$R_55BBBBM""2=11BrxzMAbhjL() ) ) )4''''"(****&!!DAq+D11Aq!Qd<)).!#)DDL,((ac	BBKaAq|(D"&13	8 	8 	8A <13>a  A%%%%>\))))=K''''=!!!,//139<<<<>""1<00AC!#I====<  ..!#)ac/AC2GGGGG<A&&q))a#a&&j1s1vv:.E-LBQK!c!ff*$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                  }|                    |          t          t          |           t          |            dgt          |           t          |           dgg dg          k    sJ |                    |          t          t          |          t          |           dgt          |          t          |          dgg dg          k    sJ t          |                    |                    t          t          | |z             t          | |z              dgt          | |z             t          | |z             dgg dg          k    sJ |                    |          ||j        z  k    sJ |                    |          ||j        z  k    sJ |                    |          ||j        z  ||j        z  z   k    sJ |j        t          ||                     t                     z
  g          k    sJ |j        t          ||                    t                     z
  g          k    sJ |
j                            |          ||z  |j        z  k    sJ |j                            |          ||z  |j        z  k    sJ |j                            |          ||z  |j        z  |||z   z  |j        z  z   k    sJ d S )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88vB#b''15 #BR!4iiiA B B B B B B5588vB#b''15 #BR!4iiiA B B B B B BAEE!HH%%#b2g,,R"Wq1Q25b2g,,BGa1P1:1< *= *= = = = =
 <<??b13h&&&&<<??b13h&&&&<<??b13hac11111 7fb2771::o.//////7fb2771::o.////// ""ad13h....""ad13h....""ad13hBGQS&@@@@@@@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                  }|
                    |	          t          t          |          dt          |           gg dt          |          dt          |          gg          k    sJ |
                    |          t          t          |          t          |           dgt          |          t          |          dgg dg          k    sJ |                    |	          t          t          |          t          |          z  t          |          t          |           t          |          z  gt          |           t          |          z  t          |          t          |          t          |          z  gt          |          dt          |          gg          k    sJ |
                    |	          ||	j        z  k    sJ |
                    |          | |
j        z  k    sJ |	                    |          | |	j        z  ||
j        z  z
  k    sJ |j        t          ||                    t                    z
  g          k    sJ |j        t          ||                    t                    z
  g          k    sJ |j                            |j                  ||
j        z  k    sJ |j                            |j                  | |
j        z  k    sJ |j                            |	          |dz  |dz  z
  |z  |
j        z  k    sJ |j                            |	          |dz  |dz  z
  |z  |dz  |dz  z   |z  z   |
j        z  k    sJ d S )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      sX    344BBA-.EFFE3usAsAsA
q1u*	B
Q$1*B
uAB
'
'
'Cr+++ESA	$3E% f
6 
6 
6B	$UE!!#v!#
7 
7 
7B 5588vE

AE

{;(yy #E

As5zz:< = = = = = = 5588vC3s88)Q7 #C#c((A6(yy* + + + + + + 5588v	S#e**	c#hhUCHH(<=
c((3u::	s3xxS#e**)<=	UQE

#% & & & & & & <<??eACi''''<<??ufQSj((((<<??ufQSj5944444 7feejjmm345555557fechhqkk12333333 <  00BqsF::::>""5#344"qs
BBBB >a  QqS2a4Z$6qs$:::::""!bd
E'9()!bd
E'9(:;<3'? ? ? ? ? ? ?r0   c                     t          d          \  } }t                      \  }}t          d          t          d          ct          t          fd           t          t          fd                               ||j        |                                ||j        |            t          t          fd           t          t          fd           t                      \  }}t          d                              |t          dz  t          t          d	z  fd
           t          d| ||j
        j         j
        	  	         t          |                    |                    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
        z  k    sJ j                            j                  |j
        |j        z   k    sJ j                            |          ||j        z  k    sJ j                                      t/          d          k    sJ j                                      ||j        z  k    sJ t                      \  }}t          d                              |dt          z  d	z  t           t          dz  fd
           t          d| ||j         j
        j
        j        z   	  	         t          |                    |                    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 |                    |          t          d          |z  dz  |j
        z  t          d          |z  dz  |j        z  z   k    sJ j                            j                  |j         |j
        z
  k    sJ j                            |                                          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                                      t/          d          k    sJ t                      \  }}t          d          t          d          c                    |t           dz  t          t          dz  fd
                               |dt          z  d	z  t           t          dz  fd
           t          d| ||j
        |j        z
  j         j
        j        z   
  
         t          |                    |                    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 |                    |          t          d          |z  dz  j
        z  t          d          |z  dz  j        z  z   k    sJ j                            j                  |j
        |j        z
  |j        z   k    sJ t          d           t          d          z   |z  dz  |j
        z  }j                            |                                          |k    sJ j                                                                      |k    sJ j                                      t/          d          k    sJ d S )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J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I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JJJJJKKK
:IIIIIIJJJQQQQ
:JJJJJJKKK
:IIIIIIJJJ!!JAq!Q&!!D1rAvr262E:::S!Q113QSD#8 8 8 8AEE!HH%%	a3q66!A%Qx#a&&'81'<=	a1d1ggSVVc!ff!459	aSVVGa#a&&j(	)A	-	/	a1d1ggc!ffs1vv!56:
q''SVVa#a&&j(	)A	-	/	00 )1 )1 1
 5:!HH= = = = <<??a$&j((((<  ..!#);;;;<A!ac'))))<D!!VAYY....<D!!QW,,,,!!JAq!Q&!!D1q2vzB3Q7???S!Q1AC4QS"qsQSy: : : :AEE!HH%%
q''CFF	Q	
q''SVVaZ	 1	$s1vvz	1AaDD1H	<	aCFFQJ	!	#c!ffqj	01Q44!8	;	= 
Q$q''DGG#Aw.277(T!WW
A	&	*	,	a3q66	A		aCFFQJ	!	#c!ffqj	01Q44!8	;	aAAJ	!	#c!ffqj	01Q44!8	;	=1> *? *? ? ? ? ? <<??d1ggkAo3d1ggkAo6KKKKKK<  ..AC%!#+====<A''))	a477a. "#.$&'c.*Qx$q''!Q-')*-+-.S-1.1 1 1 1 1 <D!!VAYY....!!JAq!Q'')?)?JD$1sQwBF3U;;;1q2vzB3Q7???S!Q113913$#d) ) ) ) AEE!HH%%	Q$q''DGG#Aw.277(T!WW
A	&	*	,
q''CFF	Q	
q''SVVaZ	 1	$s1vvz	1AaDD1H	<	aCFFQJ	!	#c!ffqj	01Q44!8	;	= 
a3q66	A		aCFFQJ	!	#c!ffqj	01Q44!8	;
q''SVVaZ	 1	$s1vvz	1AaDD1H	<	>1? *@ *@ @ @ @ @ <<??d1ggkAo6	: :::V:     <  ..!#)ac/AAAA1ggXQ1$q(13.E<A''))U2222<D!!**,,5555<D!!VAYY......r0   c            
         t          d          \  } t          d          \  }t          d| j                  }|j        j        k    s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                     |          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                     |          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                     |          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           d S )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  s?    xQ36q66AC<#a&&13,3N P  P  P r0   c                  4    t          d  j                  S r   r   r)   r$   r!   s   r.   r8   z+test_pin_joint_joint_axis.<locals>.<lambda>T  s    xQacBBB r0   c                  D    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acACiHHH r0   c            	      H    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6AC< !  !  ! r0   c            	      H    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3< !  !  ! r0   c            
      H    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!99   r0   c            	      H    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3<! ! ! 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  s   &!!DAq+D11Aq!Qd
31ad$(QS: : :C>QS    5588vA3q662JJJ #ACFF735 6 6 6 6 6 6 ,D11Aq!Qd
31ad$(TV= = =C>TV####5588vQCFF3ZZZ #A3q6624 5 5 5 5 5 5  !!JAq!Q
31aac%&SDQS: : :C>QS    5588vQ#a&&!4AwA6J)zz+ , , , , , ,!!JAq!Q
31aac%&SDQS: : :C>QS    5588vzzzAs1vvs1vv+> !3q66CFF735 6 6 6 6 6 6 ,D11Aq!Qd
: P P P P P P P Q Q Q :BBBBBCCC
:HHHHHIII
: ! ! ! ! ! ! ! " " " : ! ! ! ! ! ! ! " " " ,D11Aq!QdS!Q$& & & & 9           9 ! ! ! ! ! ! ! " " " " "r0   c                  1   t          d          \  } }t                      \  }}}}t          d|||j                    |j                             |j                  dk    sJ |j                            |           |j        k    sJ |                    |          t          g ddt          |            t          |            gdt          |            t          |           gg          k    sJ |
                    |          ||j        z  k    sJ |
                    |                                          t          |dz            k    sJ |j                            |j                  dk    sJ |j                            |j                                      |                                          dk    sJ |j                            |          dk    sJ t                      \  }}}}t          d|||j        |j                   |j                            |j                  dk    sJ |j                            |          |j        k    sJ |                    |          t          dt          |            t          |            gg ddt          |            t          |           gg          k    sJ |
                    |          ||j        z  k    sJ |
                    |                              |          ||j        z  k    sJ |
                    |                                          t          |dz            k    sJ |
                    |                              |j                  dk    sJ |j                            |          ||j        z  k    sJ |j                            |j                  |j         k    sJ |j                            |j                                      |                                          t          |           |j        z  t          |           |j        z  z   k    sJ |j                            |                              |j                  t(          dz  k    sJ t                      \  }}}}t          d|||j        |j        	           |j                            |j                  dk    sJ |j                            |          |j        k    sJ |                    |          t          g d
t          |            dt          |           gt          |           dt          |           gg          k    sJ |
                    |          ||j        z  k    sJ |
                    |                              |          ||j        z  k    sJ |
                    |                                          t          |dz            k    sJ |
                    |                              |j                  }|                    |di          dk    sJ |j                            |          dk    sJ |j                            |j                  |j        k    sJ t                      \  }}}}t          d|||j        |j        |j        |j        z              t-          |j                            |j        |j        z                       dk    sJ |j        |j        z                       |                                          t          d          |j        z  k    sJ t/          |                    |                    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        z  k    sJ |
                    |                              |                                          ||j        z  ||j        z  z   t          d          z  k    sJ |
                    |                                          t          |dz            k    sJ |
                    |                              |j        |j        z             }|                    |di          dk    sJ |j                            |                                          ||j        z  t          d          z  k    sJ |j                            |j                  |j        |j        z
  k    s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                            |                              |                                          t          d           |z  t          |           z  dz  |j        z  t          d          |z  t          |           z  dz  |j        z  z   k    sJ |j                            |                              |j                  t(          dz  k    sJ t                      \  }}}}t          d|||j        |j        |j        |j        z   |j        z
             t-          |j                            |j        |j        z   |j        z
                      dk    sJ |j        |j        z   |j        z
                      |                                          t          d          |j        z  k    sJ t/          |                    |                    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        z  k    sJ |
                    |                              |                                          ||j        z  ||j        z  z   ||j        z  z
  t          d          z  k    sJ |
                    |                                          t          |dz            k    sJ |
                    |                              |j        |j        z   |j        z
            }|                    |di          dk    sJ |j                            |                                          ||j        z  ||j        z  z   t          d          z  k    sJ |j                            |j                  |j        |j        z
  k    s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                            |                              |                                          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                  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        z   |j        z
            }t-          |          dk    sJ |j        |j        z
  |j        z                       |                                          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/          |                    |                    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        z  ||j        z  z
  ||j        z  z   t          d          z  k    sJ |
                    |                              |                                          ||j        z  ||j        z  z   ||j        z  z
  t          d          z  k    sJ |
                    |                                          t          |dz            k    sJ |
                    |                              |j        |j        z   |j        z
            }|                    |di          dk    s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        z  ||j        z  z
  k    s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                            |                              |                                          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        |j        z
  |j        z                                             dk    sJ d S )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  sP   *%%DAq  !!JAq!QS!Q!#....SD$$))))CKKNN?ac!!!!5588vzzz !CFF7SVVG4 !CFF7CFF35 6 6 6 6 6 6 <<??ae####<<??$$&&$q!t**4444<  ..!3333<  ..66q99BBDDIIII<A!####  !!JAq!QS!Q!#>>>>3QS!!Q&&&&3;;q>>QS    5588vCFF7SVVG4(yy !CFF7CFF35 6 6 6 6 6 6 <<??ae####<<??""1%%QS0000<<??$$&&$q!t**4444<<??  %%****<A!AC%''''<  ..13$6666L!!!,//77::CCEEFF13JQ#$ $ $ $<A,,QS11RT9999  !!JAq!QS!Q!#AC@@@@3QS!!Q&&&&3;;q>>QS    5588vyyy!$QCFF3 #A3q6624 5 5 5 5 5 5 <<??ae####<<??""1%%13....<<??$$&&$q!t**4444LLOO))!#..E>>1a&!!Q&&&&<A!####<  ..!#5555  !!JAq!QS!QQSaccACi) ) ) )ac''ac	2233q8888C!#Iq!!**,,Q;;;;AEE!HH%%	aT!WWHSVVOA%QxAq'89	aDGGCFFN1$d1ggc!ffnQ&67	
SVVGSVV1 * *     <<??ae####LLOO##A&&//11qsUQqsU]DGG#$ $ $ $<<??$$&&$q!t**4444LLOO))!#)44E>>1a&!!Q&&&&<A''))a!#gtAww->>>>><  ..!#);;;;L!!!,//77::CCEEa]AC$q''#a&&."213"66GGCFFN1QS !! ! ! ! LQ''**3355!WWHQJs1vva#d1ggaiA&6q&8&<<= = = =<A,,QS11RT9999!!JAq!QS!QQSaccACi!#o/ / / /ac''ac	AC8899Q>>>>C!#IO$$Q''0022d1ggackAAAAAEE!HH%%	aT!WWHSRT]]*1,	aQAX	q	 	"	aDGGCBrE	NN*1,	aQBY		!	#
q''!T!WWSQrT"W---a/	aQ2b[!!	!!	#	%1& *' *' ' ' ' ' <<??ae####<<??""1%%..00QqsUQqsU]56qsU6;<@GG5D D D D D<<??$$&&$q!t**4444LLOO))!#)AC-88E>>1a&!!Q&&&&<A''))aeaemT!WW-DDDDD<  ..!#);;;;L!!!,//77::CCEEa]AC$q''#a"Q$h--"7"9!#"==GGCBqDMM!!#AC'(( ( ( ( LQ''**3355GGAIc!bd(mm#A%ac)GGAIc!bd(mm#A%ac)** * * * <A,,QS11RT9999!!JAq!Q5>>DAqS!QQW!ac'cACi!#o sQSy130 0 0 0 S13Y_++AC!#IO<<Ee!!!!SWQS[!!!$$--//AYq[1Q446!13&!A$$q&1SRT]]?13D*Dac)IIs1r!t8}}_Q1a',-- - - - AEE!HH%%	
1a!CFF(1*	bQAX.q01Q4469	
3q2a4x==	QqTT!V	#	%	
3q2a4x==	QqTT!V	#Qs1vvXaZ!A$$q&%8	
3q2a4x==	QqTT!V	#	%	CBqDMM	!	add1f	$aA1Hoa&7!A$$q&&@	
3q66!add1f		1 *  *          <<??ququ}qu4d1gg=====<<??""1%%..00QqsUQqsU]56qsU6;<@GG5D D D D D<<??$$&&$q!t**4444LLOO))!#ac'!#+66E>>1a&!!Q&&&&LQ((**GGAIaKM!#Q	!Aac 112 2 2 2<  ..!AC%!AC%-????L!!!,//77::CCEEAc!ffHqL!!##QS(1aA1Ho/ ,,, !CBqDMM/A-.q0455 5 5 5 LQ''**3355E!GCFFN1QS 1Q3q5QAX#6q#8#<<aCE#a"Q$h--!!#%&& & & & <A""139qs?33<<>>!CCC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 d S N)r/   r   _create_aligned_interframer)   r{   r*   )r   r    r!   r$   fs        r.   test_create_aligned_frame_pir     s   !!JAq!Q(QSD!#66A3!#::::(QSD!#66A3!#::::(QSD!#66A3!#::::(QSD13Jac	BBA3!#::::(QSD13Jac	BBA3!#::::(QSD13Jac	BBA3!#::::(QSD13J,<acACi!#oNNA39ac	!!!!!!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                   	                    |          |k    sJ t          d          \  }}}t          d	| |||j                    	                    |          |k    sJ t          d          \  }}}t          t          	fd           d S )	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F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     s   %  DAq+D11Aq!QdaAqD4PPPA<46!!!!QAA'QQ(II    E ,D11Aq!QdS!Q1t   5588u+D11Aq!QdS!Q1t! ! ! !5588u+D11Aq!Qd
:FFFFFFFGGGGGr0   c                     t                      \  } t          dj        j        z             }|j        j        dk    sJ |j                            j                  j        j        z   k    sJ |j        j        k    sJ t                      \  } 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        	                    d| j        z            t          t          fd	           t          d          t          t          fd
           d S )Nr6   rm   	J_P_jointp)rm   rn   c                  Z    t          d j                                                S Nr6   r   )r   r)   	to_matrix)r$   r   r!   s   r.   r8   z'test_locate_joint_pos.<locals>.<lambda>3  s&    8CAACMM!4D4DEE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ACHH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K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!QQS13Y777E"k1111&&q|44ac	AAAA,,,,!!JAq!Q<))#qsQSy99LS!Q\!"/ / /E----,,,,!!JAq!Q
9EEEEEEG G G 	sA!!JAq!Q
:HHHHHHHIII!!JAq!Q,((a!#g66K
:KKKKKKLLL**K
:KKKKKKLLLLLr0   c                     t                      \  } }t          d                              | | j        d           t	          d          }|j        k    sJ |j                            |           dk    sJ |j        |k    sJ t          d          }t                      \  } }t          d                              | | j        |           t          t          fd           t                      \  } }t          d                              | | j        d           t          t          fd           d S )	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K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I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3!!!QS!,,,S!Q2CDDDE"&77777"--a00A5555!Q&&&&sA!!JAq!Q&{33!!!QS!,,,
:KKKKKKM M M  !!JAq!Q%k22  AC+++
:IIIIIIK K K K Kr0   c            	      R   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          ||
                    t                    z
  g          k    sJ |j        |j        j        k    sJ |j                            |j                  t%          d          k    sJ |j                            |j                  t%          d          k    sJ |j                            |j                  | |j        j        z  k    sJ |j                            |j                  | |j        j        z  k    sJ |j                            |j                  ||j        j        z  k    sJ |                    |          dk    sJ |                    |          dk    sJ  |j                    dk    sJ t                      \  }}}}t/          d          \  }}	t1          d          }
|
                    |j        |j        t6          dz             t          d||||j        j        z  |	|j        j        z  |j        j        |
          }|j        |j        j        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        z  ||j        z  z
  |	|j        z  z   k    sJ |j                            |j                  ||j        j        z  k    sJ |j                            |
          t%          d          k    sJ |                    |          dk    sJ |                    |          dk    sJ t                      \  } } }}t1          d          }
|
                    |j        |j        t6          dz             t          d||||j        j        z  |	|j        j        z  |j        j        |
          }|j        |j        j        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                  | |z
  |j        j        z  |	|j        j        z  z   k    sJ |j                            |j                  ||j        j        z  k    sJ |                    |          dk    sJ |                    |          dk    sJ d S )	Nq_S, u_Sr   r   z&PrismaticJoint: S  parent: P  child: 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6S====8q====7a<<<<=FA3KK''''8vqc{{""""6VQ]O,,,,,,<179$$$$=!!!,//6!99<<<<>""1<00F1II====>""1=11qS179_DDDD<  ..A#	/AAAA<AG$$AGI5555<<??a<<??a19;;BBBBB!!JAq!Q5>>DAq'""DQWac26***sAqq179}#$qwy=QWY)-	/ 	/ 	/A <179$$$$=!!!,//1qwy=@@@@>""1<00A	MAAAA>""1=11qS179_DDDD<  ..A#ac'AacE/AacE2IIIII<AG$$AGI5555<D!!VAYY....<<??a<<??a!!JAq!Q'""DQWac26***sAqq179}#$qwy=QWY)-	/ 	/ 	/A <179$$$$=!!!,//1qwy=@@@@>""1<00A	MAAAA>""1=11qS179_DDDD<  ..A261792Dq{2RRRRR<AG$$AGI5555<<??a<<??ar0   c                     t          d          \  } }t                      \  }}}}t          d|||j                    |j                             |j                  dk    sJ |j                            |           |j        k    sJ |                    |          t          g dg dg dg          k    sJ |j        	                    |j                  | |j        z  k    sJ |j        	                    |j                                      |          
                                |  |j        z  k    sJ |j                            |          ||j        z  k    sJ |j                            |                              |          | |j        z  k    sJ |                    |          dk    sJ |                    |          dk    sJ t                      \  }}}}t          d|||j        |j                   |j                            |j                  dk    sJ |j                            |          |j        k    sJ |                    |          t          g dg d	g dg          k    sJ |j                            |          ||j        z  k    sJ |j                            |                              |          ||j        z  k    sJ |j        	                    |j                  | |j        z  |j        z
  k    sJ |j        	                    |j                                      |          
                                | |j        z  |j        z   k    sJ |                    |          dk    sJ |                    |          dk    sJ t                      \  }}}}t          d|||j        |j        
           |j                            |j                  dk    sJ |j                            |          |j        k    sJ |                    |          t          g dg dg dg          k    sJ |j                            |          ||j        z  k    sJ |j                            |                              |          ||j        z  k    sJ |j        	                    |j                  |j        | |j        z  z   k    sJ |                    |          dk    sJ |                    |          dk    sJ t                      \  }}}}t          d|||j        |j        |j        |j        z              |j                            |j        |j        z             dk    sJ |j        |j        z                       |          t          d          |j        z  k    s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                  | dz   |j        z  |j        z
  k    s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        z   z  t          d          z  k    sJ |j                            |          ||j        z  k    sJ |                    |          dk    sJ |                    |          dk    sJ t                      \  }}}}t          d|||j        |j        |j        |j        z   |j        z
             |j                            |j        |j        z   |j        z
            dk    sJ |j        |j        z   |j        z
                      |          t          d          |j        z  k    sJ t!          |                    |                    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                  | dz   |j        z  |j        z
  k    s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        z  k    s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 |                    |          dk    s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        z   |j        z
            
                                dk    sJ |j        |j        z   |j        z
                      |          |j        |j        z
  |j        z   k    sJ t!          |                    |                    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                  |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                                      |          ||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                            |          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                            |                              |          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 |                    |          dk    sJ |                    |          dk    sJ d S )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     s   *%%DAq!!JAq!Q31t4444SD$$))))CKKNN?ac!!!!5588vzzz:::yyyABBBBBB<  ..!ac'9999<  ..66q99BBDDQSPPPP<A!ac'))))<A&&q))aR!#X5555<<??a<<??a  !!JAq!Q31qsDDDD3QS!!Q&&&&3;;q>>QS    5588vzzz999iii@AAAAAA<A!ac'))))<A&&q))QW4444<  ..!AC%!#+====<  ..66q99BBDD!#PQPSSSSS<<??a<<??a  !!JAq!Q31!#FFFF3QS!!Q&&&&3;;q>>ac!!!!5588vyyy***iii@AAAAAA<A!ac'))))<A&&q))QW4444<  ..!#!#+====<<??a<<??a  !!JAq!Q3113AC$%C!#I/ / / /3QS13Y''1,,,,C!#Iq!!T!WWQS[00005588vQ	DGG8A:q9DGGAItAwwqyRS;TV_V_V_`aaaaaa<  ..1q5!#+2CCCCC<  ..66q99a$q''!)ma>OQRQT=TW[\]W^W^_`W`abadWd=ddddd<A&&q))Q!#)_T!WW-DDDDD<A!AC%''''<<??a<<??a!!JAq!Q3113AC$%C!#IO5 5 5 53QS13Y_--2222C!#IO$$Q''47713;6666AEE!HH%%$q''!)d1ggXaZaQR1S26q''!)T!WWQY1a=OQRSTQUQUVWQWZ^_`ZaZabcZcQc1d3777(1*add1ftAwwqy>PRVWXRYRYZ[R[^_`a^b^bcd^dRd1e1g *h *h h h h h <  ..1q5!#+2CCCCC<  ..66q99	
T!WWQY	AC$q''!)AC-/$q''!)AC-?@ @ @ @<A!AC%''''<A&&q))T!WWQYq[_tAwwqy{13-NQUVWQXQXYZQZ[\Q\]^]`Q`-`````<<??a<<??a!!JAq!Q5>>DAq311QS5ae$%C!#IO%&S13Y_6 6 6 6 CGACK&&qs13wqs{33<<>>!CCCCCGACK  ##qsQSy136666AEE!HH%%1Q44%'AaDD57AaDDF1K23A$$q&!A$$q&!A$$q&1I34Q44%'1Q4461Q4461J1L *M *M M M M M <  ..	
T!WWQYq[!#Q	!AC/$q''!)A+ac/AAacEIJ J J J<  ..66q99	
QqS47719Q;	#qs1utAwwqy{':AC&??2a46DQRGGTUIVWKCWYZY\B\\] ] ] ]<A$q''!)A+ac/DGGAIaKO"Cd1ggaiPQkRSRUo"UUUUU<A&&q))T!WWQYq[_tAwwqy{13-NQUVWQXQXYZQZ[\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          ||                    t                    z
  ||                    t                    z
  g          k    sJ |j        | j        k    sJ |j                            |j                  t+          d          k    sJ |j                            |j                  t+          d          k    sJ |j                            |j                  | | j        z  k    sJ |j                            |j                  || j        z  k    sJ |j                            |           || j        z  k    sJ |                    |           || j        z  k    sJ |j        | k    sJ |j        |k    sJ |                                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          ||	                    t                    z
  ||                    t                    z
  g          k    sJ |j        d| j        z  k    sJ |j                            |j                  ||j        z  k    sJ |j                            |j                  || j        z  k    sJ |j                            |j                  | | j        z  k    sJ |j                            |j                  || j        z  || j        z  z   ||j        z  z
  k    sJ |j                            |           || j        z  ||z  |j        z  z
  k    sJ |                    |           || j        z  k    sJ d S )
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%E"FFFF	#q!	$	$B7c>>>>9>>>>8q====>VVV$45555559/000000!V++++$....&&&&6))))7ffv{{1~~5vA7NOPPPPPP=AC>""1<00F1II====?##AL11VAYY>>>>?##BO44!#EEEE<  ..&13,>>>>>a  FQSL0000<<??fqsl****1$$$$!####::<<EEEEE#L11NBB6??DAq+D11Aq!Qd	#q!B,.QW&'!#g+/AG
E 
E 
EB >VRL1111119Bx((((((!R''''$....""""2%%%%7fb2771::orFKKNN/BCDDDDDD=AG####>""1<00AG;;;;?##AL11QW<<<<?##BO44!#EEEE<  	 QS6AC</!ac'9: : : :<A"qs(R!Vac\"99999<<??b13h&&&&&&r0   c                  x   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          ||                    t                    z
  ||                    t                    z
  |	|                    t                    z
  g          k    sJ |
j        | j        k    sJ |
j        | j        | j        gk    sJ |
j                            |j                  t1          d          k    sJ |
j                            |j                  t1          d          k    sJ || j        z  || j        z  z   }|
j                            |
j                  | k    sJ |j                            |j                  |k    sJ |
j                            |           || j        z  |	| j        z  z   k    sJ |                    |           || j        z  k    sJ |
j        | k    sJ |
j        |k    sJ |
                                dk    sJ t          d          \  }}}}}}t?          d          \  }}t          d          \  } }}}}}t          d|||||g||g|| j        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          ||                    t                    z
  ||                    t                    z
  ||                    t                    z
  g          k    sJ |
j        |j        k    sJ |
j        |j        |j        gk    sJ |
j                            |j                  ||j        z  k    sJ |
j                            |j                  || j        z  k    sJ |
j                            |
j                  || j        z  || j        z  z   k    sJ |j                            |j                  || j        z  || j        z  z
  || j        z  z
  ||j        z  z
  k    sJ |j                            |           | | j        z  || j        z  z
  ||z  |j        z  z   k    sJ |                    |           || j        z  k    sJ d S )
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5FFF+H55FFF	S!Q		B7c>>>>9>>>>8q====>VVVV$<======97888888!V++++ FFF+;$<$<<<<<&&&&vvv&67777777ffv{{1~~5vA7N$v{{1~~57 8 8 8 8 8 8qs""""ac
****>""1<00F1II====?##AL11VAYY>>>>QSL6AC<'E?##BN33v====<  ..%7777>a  FQSL6AC<$?????<<??fqsl****1$$$$!####::<<@@@@@+L99BBB6??DAq+D11Aq!Qd	S!QB)+RR"#ac'q13w'+d
D 
D 
DB >VRRL1111119B/000000!R'''' FB8$4$44444&&&&vr2h//////7ffrwwqzz12

? 2771::o/ 0 0 0 0 0 0tv%%%% 00000>""1<00AG;;;;?##AL11QW<<<<?##BN33rACx"qs(7JJJJJ<  	 QS28+b13h6QS@A A A A<A2#)b13h"6!ac9I"IIIII<<??fqsl******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        t          d          z  z   	                                k    sJ |j                            |	          |	j        |	j        t          d          z  z   	                                k    sJ |j        
                    |j                  t          dz  k    s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%          |                    |	          |z
            t'          d          k    sJ d S )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  s    ,L99BBBV__FB!!JAq!QCArB8R"b!#ac!#ac	&'cAC$q''M&9%&S13a=%8		: 	: 	:A
 ?""1%%!#d1gg*=)H)H)J)JJJJJ?""1%%!#d1gg*=)H)H)J)JJJJJ?((--a77775588b!RB233s1vv====	R477(SWW$q(#b''A+6	a3r77	Q	CGGa% 7	aABK	 1	$	&
b''AtAww!c"gg+.2CGGaK%4GH	J K KE AEE!HHu,--q999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          ||
                    t                    z
  ||
                    t                    z
  |	|
                    t                    z
  g          k    sJ |
j                            |j                  t          d          k    sJ |
j                            |j                  t          d          k    sJ |
j                            |
j                  t          d          k    sJ |j                            |j                  t          d          k    sJ |j                            |           t          d          k    s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 |                    |          |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                    dk    sJ |
j        dk    sJ |
j        dk    sJ |
j        J d S )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  sm   !!JAq!Q+,<==BBBsAq!!A6S====8q====7a<<<<=FBB<0000008vr2rl++++++6VR"''!**_b2771::orBGGAJJOPPPPPP=!!!,//6!99<<<<>""1<00F1II====>""1=11VAYY>>>><  ..&));;;;<A&))++++<<??sSWW}s2ww6c"ggELRL3r77"R#b''\1QSP9 93R=2%<-- - - - - <<??rCGG|c"gg5SWWDKC#b''MCGG#b3r77l2acO: :B<"$=,, , , , , 19;;B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 |                    |	          | 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 d S )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$    sv     ''JBB**JBB!!JAq!QsAqr2rlBB<PPPA=FBB<0000008vr2rl++++++6VIII&&&&&&<<??sSWW}s2ww6c"ggEL
SWWs2wwc"gg-P5 5sSWW}r!QS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           d S )Nr  z
q0:4, u0:4r   c            	      d    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L r0   c                  l   	 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>  s=    ~Q62r2r*++VRRL-A-A C  C r0   c                  l   	 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>  s=    ~Q62r2,''RR0@)A)A C  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    sT   #12B#C#C Cc3S%3L%A%A"BBBB!!JAq!QsAq2r2,R==A=FBB<0000008vr2rl++++++!!JAq!QsAq&"b""6"6r2rl++	- 	-A=FBB<0000008vr2rl++++++!!JAq!Q
:LLLLLLLLN N N : C C C C C C C C C C C C D D D
: C C C C C C C C C C C C D D D D 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                                        d	k    sJ |	j        dk    sJ t          |                    |          | z
            t!          d
          k    sJ |                    |                              |          |k    sJ |j                            |                              |          |k    sJ t          d          \  }}}}}}t          d||gg|j        |j	        z   |j	         |j
        z   ||d	fd          }	fd}
|	j                                        d	k    sJ |	j        dk    sJ t          |                    |           |
|           z
            t!          d
          k    sJ |                    |                              |           |
|          k    sJ |j                            |                              |           |
|          k    sJ t          d          \  }}}}}}t          d||gg|j        |j	        z   |j	         |j
        z   ||dd          }	|	j                                        d	k    sJ |	j        dk    sJ t          |                    |          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 |                    |                              |          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                            |                              |          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 d S )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                 F    |                      i          S r   r   exprrF   rG   r`   rJ   rK   rb   s    r.   r8   z2test_spherical_joint_orient_body.<locals>.<lambda>  1    	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9BBB
b''CGG8c"gg%s2wwR'89
b''CGG	SWWs2ww.R83r77SWW;LL
b''CGG	c"gg	%B#b''(9	9	;	R3r77	SWWHs2ww.R3r771BSWW1LL
b''CGG	c"ggB/#b''9	9	;	< = =E bS3r77]R'(S3r77]SWW,rCGG|;<#b''\CGG+b3r77l:;= > >E 
q''R#b26k***SWW4rCR!V<L<L7LL	MN
s2ww	sSWW}r124 5 5F ,D11Aq!QdsAqr2rlBB<$%C!#IAC4!#:)- &#	7 	7 	7A ;&((((<3AEE!HHu,--q9999<<??$$Q''50000<A((++v5555+D11Aq!QdsAqr2rlBB<$%C!#IAC4!#:)- &R	M 	M 	MA: : : : : : : : :L;&((((<3AEE!HH||E':'::;;uQxxGGGG<<??$$Q''<<+>+>>>>><A((++||F/C/CCCCC+D11Aq!QdsAqr2rlBB<$%C!#IAC4!#:)- &%	9 	9 	9A ;&((((<5    AEE!HHv
b''CGG	SWWs2ww.R83r77SWW;LL	R3r77	SWW	$s2wwR'8	8	:
b''CGG8c"gg%Bx#b'''9:	R3r77	SWWHs2ww.R3r771BSWW1LL	R3r77	SWWs2ww.R8	8	:	/; (< (< < = =
 AFaI I I I <<??$$Q''6	c"gg	b3r77lSWW4rCGG|CD	c"ggB	"s2ww,	./31 ,2 ,2 2 2 2 2 <A((++v
q''R#b26k***SWW4rCR!V<L<L7LL	MN	c"gg	b3r77lR/072 03 03 3 3 3 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                                        d	k    sJ |	j        dk    sJ t          |                    |          | z
            t!          d
          k    sJ t          |                    |                              |                    |k    sJ t          |j                            |                              |                    |k    sJ fd}
t          d          \  }}}}}}t          d||gg|j        |j	        z   |j         |j
        z   ||d	fd          }	|	j                                        d	k    sJ |	j        dk    sJ t          |                    |           |
|           z
            t!          d
          k    sJ t          |                    |                              |                     |
|          k    sJ t          |j                            |                              |                     |
|          k    sJ t          d          \  }}}}}}t          d||gg|j        |j	        z   |j         |j
        z   ||dd          }	|	j                                        d	k    sJ |	j        dk    sJ t          |                    |          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          |                    |                              |          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                            |                              |          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 d S )Nr  r'   r   Tr   spacer  r5  SPACEr>   c                 F    |                      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    s4	   +L99BBB
b''CGG	c"ggB/#b''9	9	R3r77	SWW	$s2wwR'8	8#b''CGG:K	M
b''CGG	c"ggB/#b''9	9
b''CGG	c"gg	%B#b''(9	9CGG8c"gg;M	O	R3r77	SWWHs2ww.B8: ; ;E 	c"ggSWWs2ww.	./	c"ggSWWs2ww.	./"rCGG|2C1DF G GE 	b3r77l	b2B</0	aRC#b26k***R#b26k2B2B-BSWW-LL	MNP Q QF ,D11Aq!QdsAqr2rlBB<$%C!#IAC4!#:)- '3	8 	8 	8A ;'))))<3AEE!HHu,--q9999ALLOO55a8899UBBBBAL,,Q//99!<<==GGGG: : : : : : : : :L+D11Aq!QdsAqr2rlBB<$%C!#IAC4!#:)- '"b"	N 	N 	NA ;'))))<3AEE!HH||E':'::;;uQxxGGGGALLOO55a8899\\%=P=PPPPPAL,,Q//99!<<==fAUAUUUUU+D11Aq!QdsAqr2rlBB<$%C!#IAC4!#:)- '5	: 	: 	:A ;'))))<5    AEE!HHv
b''CGG	c"ggXB/#b''CGG2Cc"gg2MM	R3r77	SWW	$s2wwR'8	8	:
b''CGG8c"gg%Bx#b'''9:	R3r77	SWWHs2ww.R3r771BSWW1LL
b''CGG	c"gg	%B#b''(9	9	;	/< (= (= = > > > > > ALLOO55a886
rCGG|	sSWW}rCGG|c"gg/EEF	c"ggSWWs2ww.	./C1 <2 <2 2 3 36;AqkkB B B B AL,,Q//99!<<v	c"ggSWWs2ww.	./	c"ggSWWs2ww.	./	b3r77l	R#b''\	)	c"ggB	
  	!G" @# @# # $ $ (-Q{{	3 3 3 3 3 3r0   c                  J   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 |
                    |          t          d          k    sJ |j                            |j                  t          d          k    sJ |j                            |j                  t          d          k    sJ |j                            |j                  t          d          k    sJ |j                            |j                  t          d          k    sJ |j                            |j                  t          d          k    sJ |                    |          dk    sJ |                    |          dk    sJ |                                dk    sJ t                      \  }}}}t+          d          \  }}t-          d          }|                    |j        |j        t2          dz             t          d||||j        j        z  ||j        j        z  |	          }|j                            |j                  ||j        j        z  k    sJ |j                            |j                  ||j        j        z  k    sJ |j                            |j                  t          d          k    sJ |j                            |j                  | |j        z  ||j        z  z   k    sJ |j                            |j                  t          d          k    sJ |j                            |          t          d          k    sJ |                    |          dk    sJ |                    |          dk    sJ |j        |j        k    sJ t9          ||           d S )
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  -  s   !!JAq!Q#q!A6S====8q====7a<<<<=FHH$$$$8vxx6VAq"%%'''''5588s1vv=!!!,//6!99<<<<>""1<00F1II====>""1=11VAYY>>>><  ..&));;;;<AG$$q		1111<<??a<<??a99;;=====!!JAq!Q5>>DAq'""DQWac26***#q!!agi-!')mt	E 	E 	EA =!!!,//1qwy=@@@@>""1<00A	MAAAA>""1=11VAYY>>>><  ..A#)a!#g2EEEEE<AG$$q		1111<D!!VAYY....<<??a<<??a3!#::::Ar0   c            
         t          d          \  } }t                      \  }}}}t                      5  t          d|||j                    d d d            n# 1 swxY w Y   |j                             |j                  dk    sJ |j                            |           |j        k    sJ |                    |          t          g ddt          |            t          |            gdt          |            t          |           gg          k    sJ |                    |          ||j        z  k    sJ |                    |                                          t          |dz            k    sJ t                      \  }}}}t                      5  t          d|||j        |j        z              d d d            n# 1 swxY w Y   |j                            |j        |j        z             dk    sJ |j                            |          |j        |j        z   t          d          z  k    s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 |                    |          t!          d          k    sJ d S )	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  s5   *%%DAq!!JAq!Q		!	! - -at,,,,- - - - - - - - - - - - - - -SD$$))))CKKNN?ac!!!!5588vzzz !CFF7SVVG4 !CFF7CFF35 6 6 6 6 6 6 <<??a!#g%%%%<<??$$&&$qAv,,6666!!JAq!Q		!	! 9 9sAqacACi88889 9 9 9 9 9 9 9 9 9 9 9 9 9 9CqsQSy))Q....3;;q>>acACi477222225588vQ!T!WWq[!<!%a1d1ggk1=yyyJ K K K K K K<<??fQii''''''s#   AA!A!G		GGc            	      2   t                      \  } }}}t                      5  t          d||| j        | j        z   |j        |j        z
            }d d d            n# 1 swxY w Y   |j                            |j                  | j        | j        z   k    sJ |j	                            |j                  |j        |j        z
  k    sJ t                      \  } }}}t                      5  t          d||| j        | j        z   |j        |j        z
            }d d d            n# 1 swxY w Y   |j                            |j                  | j        | j        z   k    sJ |j	                            |j                  |j        |j        z
  k    sJ d S )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  s   !!JAq!Q		!	! 2 2sAq139'(sQSy2 2 22 2 2 2 2 2 2 2 2 2 2 2 2 2 2 $$Q\22acACi?????##AL11QS13Y>>>>!!JAq!Q		!	! ; ;QAC!#I01ac	; ; ;; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ''55qsBBBB&&q|44ac	AAAAAAs#   /AA!$A!-/D((D,/D,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 9 9 9 9 9 ? ? ? ? ? ? ? ? C C C C C C C C C C C C % % % % % %M M M M M M M M M M M M M M M M M M M M M M 0 / / / / / > > > > > > > > > > ? ? ? ? ? ? ? ? 
 
 
 
9 9 9,O ,O ,O^6& 6& 6&r%A %A %AP,? ,? ,?^I/ I/ I/X5" 5" 5"pLD LD LD^" " "$H H H.M M M<K K K03  3  3 l\  \  \ ~.' .' .'b4+ 4+ 4+n: : :6  6) ) )D D D293 93 93x;3 ;3 ;3|$ $ $N( ( (.B B B B Br0   