
    gB                        d dl mZ d dlmZ d dlmZ d dlmZ d dlm	Z	m
Z
 d dlmZmZmZmZ d dlmZ d dlmZmZmZmZmZmZmZmZmZmZmZ d d	lmZ d d
l m!Z!m"Z"m#Z# d dl$m%Z%m&Z& ej'        Z(d*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& ZBd' ZCd( ZDd)S )+    )
expand_mul)pi)S)sqrt)cossin)Matrixsimplifyeyezeros)symbols)dynamicsymbols	RigidBodyParticleJointsMethodPinJointPrismaticJointCylindricalJointPlanarJointSphericalJoint	WeldJointBodyJoint)VectorReferenceFramePoint)raiseswarns_deprecated_sympyFc                 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          d/var/www/html/ai-engine/env/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_joint.py_generate_bodyr1      s    sAsA#QA#Q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   r5   r4   s   r0   <lambda>ztest_Joint.<locals>.<lambda>#   s    eC77 r2   )r   r   	TypeErrorr9   s   @@r0   
test_Jointr<       sA    x  FgE
97777788888r2   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:4r8      r)   )offset   uT)number_singleqc                  4                          gd          S )Nr>   _fill_coordinate_listr8   q0q1s   r0   r:   z,test_coordinate_generation.<locals>.<lambda>F       q66BxCC r2   c                  8                          d gdd          S )Nr)   rA   rE   )r8   u0u1s   r0   r:   z,test_coordinate_generation.<locals>.<lambda>G   s    q66B~q#NN r2   c                  4                          gd          S )Nr@   rE   rG   s   r0   r:   z,test_coordinate_generation.<locals>.<lambda>H   rJ   r2   c                  N                          t          d          gd          S )NrI   r)   )rF   r   )r8   rH   s   r0   r:   z,test_coordinate_generation.<locals>.<lambda>J   s"    a55r74==6I1MM r2   c                  :                          z   gd          S )Nr)   rE   rG   s   r0   r:   z,test_coordinate_generation.<locals>.<lambda>K   s    a55rBwmQGG r2   c                  .    t          d d d g          S r7   r   )r&   r#   q1js   r0   r:   z,test_coordinate_generation.<locals>.<lambda>Q   s    ~c1a#tT9JKK r2   c                  0    t          d g          S )Nr8   )speedsrR   )r&   r#   rL   rM   s   r0   r:   z,test_coordinate_generation.<locals>.<lambda>R   s    ~c1aRMMM r2   )
r   r1   r   rF   r	   r   
ValueErrorr;   difft)rC   rA   qjujq0jq2jq3ju0ju1ju2ju3jq2q3u2u3_r&   r8   r#   rH   rI   rS   rL   rM   s                   @@@@@@@@r0   test_coordinate_generationrg   &   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Nr2   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_Jr8   r   z PinJoint: J  parent: P  child: CP1C1r'   r)   J1parent_pointchild_point
joint_axisparent_interframeq, uTro   rp   ro   rp   rr   child_interframerq   )&r   r   r   r   namer4   r5   coordinatesr	   rU   kdesrW   rX   rq   r%   r+   rp   pos_from
masscenterr   ro   _child_pointrr   ru   __str__r   r*   r,   r   z_joint_axis_parent_pointr1   	locatenewvelexpressr   r   )r#   r&   lmrC   rA   Pjrk   rl   r.   rm   r!   r"   r/   ro   rp   r8   s                    r0   test_pin_jointr   U   s   #A#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****!'))))::<<=====	4B	4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, ., , , , ,$&&&&%%%%%%r2   c                     t          d          \  } }t          d          }t          d          }t          d|           }t          d          \  }}t	          d|||||||j        z            }|j        |k    sJ |j        j        dk    sJ |j	        
                    |j	                  | |j        z  k    sJ |                    |j                  t          g d	d
t          |          t          |          gd
t          |           t          |          gg          k    sJ |j	                            |j                  | |z  |j        z  k    sJ t          d          }t          d          }t          d          }t          d|           }t          d          \  }}t	          d||||||||j        z  |j        	  	        }|j        |j        j        k    sJ |                    |j                  t          t          |          t          |          d
gt          |           t          |          d
gg dg          k    sJ |j	                            |j                  d
k    sJ |j	                            |j                  ||z  |j        z  k    sJ t          d          \  }	}
}}}}d |	|f|
|f||ffD             }t          d          t          d          }}t'          d||||          }|j                            |j                  t)          d          k    sJ |j	        
                    |j	                  ||j        j        z  k    sJ |j	                            |j                  d
k    sJ |j	                            |j                  ||j        j        z  k    sJ t          d          t          d          }}t          d          }t+          d|||	|
|||||j        z  |j        
  
        }|j                            |j                  t          t          |	          d
t          |	          gg dt          |	           d
t          |	          gg          k    sJ |j	        
                    |j	                  ||j        z  |
|j        z  z   k    sJ |j	                            |j                  ||j        z  k    sJ |j	                            |j                                      |          | |j        z  ||z  |j        z  z
  k    sJ t          d          t          d          }}t          d          }t/          d|||	|
|g|||g|||j        z  	  	        }|j        }|j                            |j                  t          g d	d
t          |	          t          |	           gd
t          |	          t          |	          gg          k    sJ |j	        
                    |j	                  | |j        z  |
|j        z  z   ||j        z  z   k    sJ |j	                            |j                  ||z  |j        z  ||j        z  z   ||j        z  z   k    sJ t          d          t          d          }}t          d          t          d          }}t1          d||||||j        z  ||j        z            }|                    |          t)          d          k    sJ |j	        
                    |j	                  ||j        z  ||j        z  z
  k    sJ |j	                            |j                  d
k    sJ d S )Nm lr&   r#   )massrs   r8   ru   rp   	J_P_framer>   r   r   r   )rr   ru   rp   rq   r   r   r>   z	q1:4 u1:4c                 J    i | ] \  }}|                     t                    |!S  )rW   rX   ).0qiuis      r0   
<dictcomp>z/test_particle_compatibility.<locals>.<dictcomp>   s(    MMMFBRMMMr2   r@   P_frame)rr   ro   rq   r   r>   r   C_frame)rr   ru   ro   rp   )r   r   r   r   r   r,   ru   rr   rv   rz   ry   dcmr	   r   r   r   r}   rq   r+   r   r   r   xreplacer   r   )r   r   r   r#   r&   rC   rA   r8   r   rI   rb   rc   rM   rd   re   	qdot_to_us                   r0   test_particle_compatibilityr      s   5>>DAqS!!GA1A&!!DAqaAq7]	, 	, 	,A((((#{2222<  ..1"wy.@@@@;;q*++vyyy893q663q667J89CFF7CFF7K7M 0N 0N N N N N <A/00QBFWY4FFFFFS!!GS!!GA1A&!!DAqaAqG")q79}#I	' 	' 	'A <1.00000;;q*++vAA7J9<QQ7K7@yy7B 0C 0C C C C C <A/00A5555<A/00AEGI4EEEEE+K88BBBMMBx"bB8.LMMMIC==(3--qAsAq!Q''A""1#566#a&&@@@@<  ..!a6I6K2KKKKK<A/00A5555<A/00A8K8M4MMMMMC==(3--qAY''GaBBg&'')m		K 	K 	KA""1#566&	R!SWWyyyCGG8QB*@BB ;C ;C C C C C<  ..!gi-"wy.2PPPPP<A/00BNBBBB<A.//88CC	gi!b&79,,. . . . C==(3--qAY''GCArB8R"b%,!gi-	I 	I 	IA!G""1#566&		As2wwR)As2wwB+@BB ;C ;C C C C C<  ..	
WYgi'"wy.8: : : :<A/00	BR')^+b79n<> > > > C==(3--qA%i00.2K2KWG#q!w#*WY')m	- 	- 	-A ;;w3q66))))<  ..!gi-!gi-2OOOOO<A/00A555555r2   c            
         t          d          \  } }t          d          }t                      5  t          d          }t          d| |          }d d d            n# 1 swxY w Y   t	          d          \  }}t          d||||||j        z             |j        |k    sJ |j        j        dk    sJ |j	        
                    |j	                  | |j        z  k    sJ |j                            |j                  t          g d	d
t          |          t          |          gd
t          |           t          |          gg          k    sJ |j	                            |j                  | |z  |j        z  k    sJ d S )Nr   r&   r#   r   r%   rs   r8   rp   r   r   r   )r   r   r   r   r   r   r,   r%   rv   rz   ry   r   r	   r   r   r   r}   )r   r   r   r#   r&   rC   rA   s          r0   test_body_compatibilityr      s   5>>DAqS!!G		!	! - -II1G,,,- - - - - - - - - - - - - - - &!!DAqS!Q1!gi-88887g7<9$$$$<  ..1"qs(::::7;;qw6999,-s1vvs1vv+>,-AwA+?+A $B $B B B B B <AG$$Q444444s   "AA"%A"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 u2r   r!   r"   Br&   r$   r#   r%   r   Rrm   )rU   rw   rp   rq   J2r   r   )r   r   r   r   r   r+   r%   r}   r   r	   r   r   r
   
ang_vel_inrx   rW   rX   rz   r   r,   )rI   rb   rM   rd   r   r   r!   r"   r   r&   PartPPartRrm   r   s                 r0   test_pin_joint_double_pendulumr      s   G$$FBG$$FB5>>DAqsAsAsA#QAc+++Ec+++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q)I25b2g,,BGa1P1:)< "= "= = = = =
 <<??b13h&&&&<<??b13h&&&&<<??b13hac11111 7fb2771::o.//////7fb2771::o.////// ""ad13h....""ad13h....""ad13hBGQS&@@@@@@@r2   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   plater   r&   r$   rm   )rw   rU   rp   rq   r   )rw   rU   ro   rq   r   r   r   )r   r   r   r   r   r}   r,   r   r	   r   r   r   rx   rW   rX   rz   ry   r   r+   )mAmBlAlBhthetaphiomegaalphar!   r"   r   lCr   r   r&   rm   r   s                     r0   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
E
,
,
,CgBa000E#Q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'? ? ? ? ? ? ?r2   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 )Nrs   r.   r/   c                  *    t          d           S Nr8   rr   r   r&   r#   r.   s   r0   r:   z+test_pin_joint_interframe.<locals>.<lambda>D      xQTJJJ r2   c                  *    t          d           S Nr8   ru   r   r&   r/   r#   s   r0   r:   z+test_pin_joint_interframe.<locals>.<lambda>E      xQDIII r2   c                  *    t          d           S r   r   r   s   r0   r:   z+test_pin_joint_interframe.<locals>.<lambda>I  r   r2   c                  *    t          d           S r   r   r   s   r0   r:   z+test_pin_joint_interframe.<locals>.<lambda>J  r   r2   r   r@   xyzr8   )ro   rp   rr   rq   g      r)      r   )ro   rp   ru   rq   r>   rt   )r   r1   r   r   rV   r*   r}   orient_body_fixedr   r   r+   r,   r
   r   r	   r   r   r   r   r   rz   ry   r   r   r   )	rC   rA   r!   r"   N_v_Cr&   r/   r#   r.   s	        @@@@r0   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	-	/	(0 !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	;	=)> "? "? ? ? ? ? <<??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	<	>)? "@ "@ @ @ @ @ <<??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......r2   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 )Nrs   Tr8   rr   ru   rq   r   r   r   )r   r   r   r   r   r   c            	          t          d t                    j        z  t                    j        z  z             S Nr8   )rq   )r   r   r+   r   r,   r&   r!   r#   rC   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s?    xQ36q66AC<#a&&13,3N P  P  P r2   c                  4    t          d  j                  S r   r   r+   r&   r#   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s    xQacBBB r2   c                  D    t          d j         j        z             S r   r   r+   r,   r   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s     xQacACiHHH r2   c            	      H    t          d j         j        z             S Nr8   r   r   r&   r/   r#   r.   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s,    xQTD6AC< !  !  ! r2   c            	      H    t          d j        j        z             S r   r   r   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s,    xQTD3< !  !  ! r2   c            
      H    t          d t          d                    S )Nr8   r   r   )r   r   r   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s*    hQTD!99   r2   c            	      H    t          d j        j        z             S r   )r   r,   r   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s,    hQTD3<! ! ! r2   )r   r1   r   r,   rq   r   r	   r   r   r}   r+   r   rV   	Exception)	rA   r"   pinr&   r/   r!   r#   r.   rC   s	      @@@@@@r0   test_pin_joint_joint_axisr     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 ! ! ! ! ! ! ! " " " " "r2   c                  2   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                      \  }}}}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
            }t-          |          dk    sJ |j        |j        z
  |j        z                       |                                          dt          |           z  dz  t1          d          dz  z
  |j        z  t1          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  t1          d          dz  z
  |j        z  z   k    sJ t          |                    |                    t          t1          d          dz  dt          |           z  dz  z
  dt          | t(          dz  z             z  dz  t1          d          dz  z
  dt          | t(          dz  z             z  dz  t1          d          dz  z   gdt          | t(          dz  z             z  dz  t1          d          dz  z   dt          |           z  dz  t1          d          dz  z
  dt          | t(          dz  z             z  dz  t1          d          dz  z   gdt          | t(          dz  z             z  dz  t1          d          dz  z
  dt          | t(          dz  z             z  dz  t1          d          dz  z   dt          |           z  dz  t1          d          dz  z
  gg          k    sJ |
                    |          ||j        z  ||j        z  z
  ||j        z  z   t          d          z  z
                                  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 )Nrj   r8   r   r   r   r)   r   r   rr   ro   r   r>   ro   rp   ru   r@   r   r         m nro   rp   ru   rr   r   )r   r1   r   r+   angle_betweenr   r   r	   r   r   r   	magnituder   rz   ry   r
   r   r,   crossr}   r   r   r   r   r   dot)	rC   rA   r!   r"   r#   r&   angler   ns	            r0   test_pin_joint_arbitrary_axisr     st   *%%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) " "     <<??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[!!	!!	#	%)& "' "' ' ' ' ' <<??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&!!**,,1111<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		) "  "          LLOOququ}qu4d1gg==GGIIIII<<??""1%%..00QqsUQqsU]56qsU6;<@GG5D D D D D<<??$$&&$q!t**4444LLOO))!#ac'!#+66E>>1a&!!**,,1111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Cr2   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)r1   r   _create_aligned_interframer+   r}   r,   )r!   r"   r#   r&   fs        r0   test_create_aligned_frame_pir   R  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	!!!!!!r2   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 uTr8   )rr   ru   r   r   r   c                  :    t          d j        z            S r   )r   r}   r   s   r0   r:   z%test_pin_joint_axis.<locals>.<lambda>x  s    xQa!#gFFF r2   )r   r1   r   rq   r+   r	   r   r   r}   r   r   rV   )
rA   r"   r.   r/   r8   N_R_Ar&   r!   r#   rC   s
         @@@@r0   test_pin_joint_axisr   d  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Gr2   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 )Nr8   ro   	J_P_jointp)ro   rp   c                  Z    t          d j                                                S Nr8   r   )r   r+   	to_matrix)r&   r!   r#   s   r0   r:   z'test_locate_joint_pos.<locals>.<lambda>  s&    8CAACMM!4D4DEEE r2   rC   c                  :    t          d j        z            S r   r   r   s   r0   r:   z'test_locate_joint_pos.<locals>.<lambda>  s    xQACHHH r2   c                  *    t          d           S Nr8   r   r   r&   r#   rp   s   r0   r:   z'test_locate_joint_pos.<locals>.<lambda>      xQ{KKK r2   c                  *    t          d           S r  r   r  s   r0   r:   z'test_locate_joint_pos.<locals>.<lambda>  r  r2   )r1   r   r,   r}   ro   rv   ry   rz   rp   r   r   r;   r   rV   r   )r"   jointro   r&   r!   r#   rp   rC   s      @@@@@r0   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Lr2   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>   r8   r   r   rC   c                  *    t          d           S r   r   )r&   r#   rr   s   r0   r:   z)test_locate_joint_frame.<locals>.<lambda>  s    8CA9JKKK r2   c                  *    t          d           S r   r   )r&   r#   ru   s   r0   r:   z)test_locate_joint_frame.<locals>.<lambda>  s    8CA8HIII r2   )r1   r   r*   r}   r   rr   r   ru   r   r   rV   )r!   r"   r  rC   r&   r#   ru   rr   s       @@@@r0   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r2   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
  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 |j                            |j                  dk    sJ |j                            |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 |j                            |j                  dk    sJ |j                            |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 |j                            |j                  dk    sJ |j                            |j                  dk    sJ d S )	Nq_S, u_Sr   r   z&PrismaticJoint: S  parent: P  child: Cri   r'   r)   rn   )r1   r   r   rv   r4   r5   rw   r	   rU   rx   rW   rX   rq   r%   r+   rp   ry   rz   r   ro   r   r   r|   r   r   r*   r,   r   r}   )rf   r#   r&   rC   rA   r   r!   r"   r   r   r.   s              r0   test_prismatic_jointr    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7ag&&!++++7ag&&!++++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#)a!#g2EAC2OOOOO<AG$$AGI5555<D!!VAYY....7ag&&!++++7ag&&!++++!!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7ag&&!++++7ag&&!++++++r2   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  z
  
                                d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
  z
  
                                d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
  z
  
                                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   z
  
                                d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
  z
                      |          
                                d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   z
  
                                d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   z
  
                                d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
  z
  
                                d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   r1   r   r+   r   r   r   r	   rz   ry   r
   r   r   r,   r   r}   r   r   )rC   rA   r!   r"   r#   r&   r   r   s           r0   #test_prismatic_joint_arbitrary_axisr    sB   *%%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_--6688A====S13Y_%%a((47713;6@@BBaGGGGAEE!HH$q''!)d1ggXaZa)K26q''!)T!WWQY1a=OQRSTQUQUVWQWZ^_`ZaZabcZcQc1d3777(1*add1ftAwwqy>PRVWXRYRYZ[R[^_`a^b^bcd^dRd1e)g "h "h h h h h <  ..1q5!#+2CCCCCL!!!,//77::
d1ggai-!
QS	 47719QS=	047719QS=	@BCK8::QRS S S S<A!AC%''''LQ''**Q	!AC$q''!)A+ac/)DGGAIaKO;=>FhjjI I I<<??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SWQS[!!!$$ac	AC8BBDDIIIIAEE!HH1Q44%'AaDD57AaDDF)C23A$$q&!A$$q&!A$$q&1I34Q44%'1Q4461Q4461J)L "M "M M M M M L!!!,//	
T!WWQYq[!#Q	!AC/$q''!)A+ac/AAacEIKgajjq) ) ) ) L!!!,//77::	
QqS47719Q;	#qs1utAwwqy{':AC&??	Aa$q''!)A+	qs"	#$%-XZZ15 5 5 5 LQ''**Q	!AC$q''!)A+ac/)DGGAIaKO;=>FhjjAN N N NLQ''**!WWQYq[_tAwwqy{13.a1QS@BCK8::QRS S S S<<??a<<??ar2   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_Jr8   r   z(CylindricalJoint: J  parent: P  child: Cz
q0:2, u0:2l, mTr)   )rotation_coordinaterotation_speedtranslation_speedro   rp   rr   ru   rq   ) r1   r   r   rv   r4   r5   rw   r	   rU   r  translation_coordinater  r  rx   rW   rX   rq   r+   rp   ry   rz   r   ro   r{   r   r   rr   ru   r|   r   r,   r}   )r!   r"   r#   r&   q0_defq1_defu0_defu1_defCjrH   rI   rL   rM   r   r   r.   r/   s                    r0   test_cylindrical_jointr  K  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&&&&&&r2   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_Jr8   r   z#PlanarJoint: J  parent: P  child: C
q0:3, u0:3r  T)r  planar_coordinatesplanar_speedsro   rp   rr   ru   ) r1   r   r   rv   r4   r5   rw   r	   rU   r  r   r  r!  rx   rW   rX   rotation_axisr+   planar_vectorsr,   r}   rp   ry   rz   r   ro   r   r   rr   ru   r|   r   )r!   r"   r#   r&   r  r  q2_defr  r  u2_defr  r_P_CrH   rI   rb   rL   rM   rd   r   r   r.   r/   s                         r0   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******r2   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:3r8   r@   )ro   rp   rr   ru   r   r   r)   r   g      ?r>   g      ?)r   r   r1   r   r}   r,   r   r"  r   	normalizer   r   r   r   r   r	   r   r   r
   r   )rH   rI   rb   rL   rM   rd   l1l2r!   r"   r#   r&   r8   r   s                 r0   test_planar_joint_advancedr,    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111111r2   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{   )r1   r   r   rv   r4   r5   rw   r	   rU   rx   rW   rX   rp   ry   rz   r   ro   r   r   r   r   r+   r,   r}   r|   	_rot_type
_rot_order_amounts)r!   r"   r#   r&   rH   rI   rb   rL   rM   rd   r   s              r0   test_spherical_jointr4    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:r2   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   )rw   rU   )r   r   r   )r   r1   r   rw   r	   rU   rx   r   r   r   r+   r,   r}   )rH   rI   rb   rL   rM   rd   r!   r"   r#   r&   r   s              r0   /test_spherical_joint_speeds_as_derivative_termsr6    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)) ) ) ) ) ) )r2   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#   rH   rI   rL   s   r0   r:   z-test_spherical_joint_coords.<locals>.<lambda>	  s+    >#q!VRH-=-=vrd||LL r2   c                  l   	 t          d t          g          t          g                    S r9  r:  )	r&   r#   rH   rI   rb   rc   rL   rM   rd   s	   r0   r:   z-test_spherical_joint_coords.<locals>.<lambda>  s=    ~Q62r2r*++VRRL-A-A C  C r2   c                  l   	 t          d t          g          t          g                    S r9  r:  )	r&   r#   rH   rI   rb   rL   rM   rd   u4s	   r0   r:   z-test_spherical_joint_coords.<locals>.<lambda>  s=    ~Q62r2,''RR0@)A)A C  C r2   )r   r1   r   rw   r	   rU   r   rV   )q0sq1sq2su0su1su2sr!   r"   r   r&   r#   rH   rI   rb   rc   rL   rM   rd   r=  s            @@@@@@@@@@r0   test_spherical_joint_coordsrD    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r2   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 t          |                    |                              |          |z
            t!          d
d          k    sJ t          |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 t          |                    |                              |           |
|          z
            t!          d
d          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                    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          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	          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   bodyr0  rw   rU   ro   rp   rr   ru   rot_type	rot_orderr/  r@   r>   	rw   rU   ro   rp   rr   ru   rH  amountsrI  c                 F    |                      i          S r   r   exprrH   rI   rb   rL   rM   rd   s    r0   r:   z2test_spherical_joint_orient_body.<locals>.<lambda>0  1    	RRRRRR8!: !: r2   BodYyxz)r   r	   r   r   r   r   r1   r   r+   r,   r}   r1  upperr2  r
   r   r   r   r   rz   r   r   N_w_AN_v_Cor!   r"   r#   r&   r.   r/   r   switch_orderrH   rI   rb   rL   rM   rd   s              @@@@@@r0    test_spherical_joint_orient_bodyrX    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1111ALLOO--a005899U1a[[HHHHAL$$Q''11!4455????+D11Aq!QdsAqr2rlBB<$%C!#IAC4!#:)- &R	M 	M 	MA: : : : : : : : :L;&((((<3AEE!HH||E22233uQxx????ALLOO--a00<<3F3FF  !&q!- - - -AL$$Q''11!4455f9M9MMMMM+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 ALLOO--a006	c"gg	b3r77lSWW4rCGG|CD	c"ggB	"s2ww,	./;1 42 42 2 3 36;AqkkB B B B AL$$Q''11!4455
q''R#b26k***SWW4rCR!V<L<L7LL	MN	c"gg	b3r77lR/0A2 :3 :3 3 3 3 3 3 3r2   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   spacer0  rG  SPACEr@   c                 F    |                      i          S r   rM  rN  s    r0   r:   z3test_spherical_joint_orient_space.<locals>.<lambda>h  rP  r2   rJ  SPaCezxyr>   )r   r	   r   r   r   r   r1   r   r+   r}   r,   r1  rS  r2  r
   r   r   r   r   rz   r   rT  s              @@@@@@r0   !test_spherical_joint_orient_spacer_  N  s0	   +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1111ALLOO--a0011U::::AL$$Q''11!4455????: : : : : : : : :L+D11Aq!QdsAqr2rlBB<$%C!#IAC4!#:)- '"b"	N 	N 	NA ;'))))<3AEE!HH||E22233uQxx????ALLOO--a0011\\%5H5HHHHHAL$$Q''11!4455f9M9MMMMM+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--a006
rCGG|	sSWW}rCGG|c"gg/EEF	c"ggSWWs2ww.	./;1 42 42 2 3 36;AqkkB B B B AL$$Q''11!44v	c"ggSWWs2ww.	./	c"ggSWWs2ww.	./	b3r77l	R#b''\	)	c"ggB	
  	!?" 8# 8# # $ $ (-Q{{	3 3 3 3 3 3r2   c                     t                      \  } } }}t          d||          }|j        dk    sJ |j        |k    sJ |j        |k    sJ |j        t                      k    sJ |j        t                      k    sJ |j        t          ddg           j	        k    sJ |j
                            |j
                  t          d          k    sJ |j                            |j                  t!          d          k    sJ |j                            |j                  t!          d          k    sJ |j                            |j                  t!          d          k    sJ |j                            |j                  t!          d          k    sJ |j                            |j
                  t!          d          k    sJ |j
                            |j
                  dk    sJ |j
                            |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 |j
                            |j
                  dk    sJ |j
                            |j
                  dk    sJ |j        |j        k    sJ t9                      5  t;          ||           d d d            d S # 1 swxY w Y   d S )
NWr>   r   r@   z!WeldJoint: W  parent: P  child: Cri   r'   r)   )ro   rp   rr   )r1   r   rv   r4   r5   rw   r	   rU   rx   Tr%   r   r   rp   ry   rz   r   ro   r   r   r|   r   r   r*   r,   r   r+   r}   r   r   )	rf   r#   r&   ra  r!   r"   r   r   r.   s	            r0   test_weld_jointrc    sX   !!JAq!Q#q!A6S====8q====7a<<<<=FHH$$$$8vxx6VAq"%%'''''7;;qw3q66))))=!!!,//6!99<<<<>""1<00F1II====>""1=11VAYY>>>><  ..&));;;;<AG$$q		11117ag&&!++++7ag&&!++++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....7ag&&!++++7ag&&!++++3!#::::		!	!  Q                 s   Q11Q58Q5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 )	Nrj   r8   )
child_axisr   r   r)   )parent_axisr   )r   r1   r   r   r+   r   r   r   r	   r   r   r   r   r   r   r,   r   )rC   rA   r!   r"   r#   r&   s         r0   !test_deprecated_parent_child_axisrg    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 )Nr8   )parent_joint_poschild_joint_pos)r1   r   r   r+   r,   r}   ro   ry   rz   rp   r   )r!   r"   r#   r&   r   sliders         r0   test_deprecated_joint_posrl    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)Esympy.core.functionr   sympy.core.numbersr   sympy.core.singletonr   (sympy.functions.elementary.miscellaneousr   (sympy.functions.elementary.trigonometricr   r   sympyr	   r
   r   r   sympy.core.symbolr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   sympy.physics.mechanics.jointr   sympy.physics.vectorr   r   r   sympy.testing.pytestr   r   _trX   r1   r<   rg   r   r   r   r   r   r   r   r   r   r   r  r  r  r  r  r'  r,  r4  r6  rD  rX  r_  rc  rg  rl  r   r2   r0   <module>ry     ss   * * * * * * ! ! ! ! ! ! " " " " " " 9 9 9 9 9 9 ? ? ? ? ? ? ? ? . . . . . . . . . . . . % % % % % %D D D D D D D D D D D D D D D D D D D D D D D D D D 0 / / / / / > > > > > > > > > > ? ? ? ? ? ? ? ? 
 
 
 
9 9 9,O ,O ,O^6& 6& 6&rF6 F6 F6R5 5 5"%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a  a  a H.' .' .'b4+ 4+ 4+n2 2 26  6) ) )D D D2:3 :3 :3z;3 ;3 ;3|% % %P( ( (.B B B B Br2   