+
    iB                        ^ RI Ht ^ RIHt ^ RIHt ^ RIHt ^ RIH	t	H
t
 ^ RIHtHtHtHt ^ RIHt ^ RIHtHtHtHtHtHtHtHtHtHtHt ^ R	IHt ^ R
I H!t!H"t"H#t# ^ RI$H%t%H&t& ]PN                  t(R)R lt)R t*R t+R t,R t-R t.R t/R t0R t1R t2R t3R t4R t5R t6R t7R t8R t9R t:R t;R t<R  t=R! t>R" t?R# t@R$ tAR% tBR& tCR' tDR(# )*    )
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_sympyc                 >   \        R 4      p\        R4      p\        RVR7      p\        RVR7      pV '       dc   \        R4      \        R4      reVP                  WP                  \        4       VP                  W"P
                  \        ) ^,          4       WW4WV3# WW43# )NAPframeCP_intC_int)r   r   orient_axisxr   y)
interframer"   r#   r$   r'   PintCints   &      f/var/www/html/photoedit/myenv/lib/python3.14/site-packages/sympy/physics/mechanics/tests/test_joint.py_generate_bodyr1      s    sAsA#QA#QA#G,nW.EdCC$CC"q)Q4%%:    c                  `   a a \        R 4      o\        R4      o \        \        V V3R l4       R# )parentchildc                     < \        R SS 4      # )Jr   r5   r4   s   r0   <lambda>test_Joint.<locals>.<lambda>#   s    eC7r2   N)r   r   	TypeErrorr8   s   @@r0   
test_Jointr<       s"    x FgE
978r2   c            	        aaaaaaaa \        R 4      w  rr#\        R4      w  porVrxr\        R4      w  ooroor\        4       w   poo\        RSS4      oSP                  V ^4      \	        V .4      8X  g   Q hSP                  V.^4      \	        V.4      8X  g   Q hSP                  V.^^R7      \	        V.4      8X  g   Q hSP                  R^4      \	        V.4      8X  g   Q hSP                  R.^4      \	        V.4      8X  g   Q hSP                  SRR.^4      \	        SSV.4      8X  g   Q hSP                  R^4      \	        VSV.4      8X  g   Q hSP                  . ^4      \	        VSV.4      8X  g   Q hSP                  . ^^R7      \	        SWV.4      8X  g   Q hSP                  SRV.^^R7      \	        SW\.4      8X  g   Q hSP                  R^^R7      \	        WV.4      8X  g   Q hSP                  R^R4      \	        V.4      8X  g   Q hSP                  . ^R4      \	        WxV	.4      8X  g   Q hSP                  R^RR7      \	        V.4      8X  g   Q hSP                  . ^R^R4      \	        V	.4      8X  g   Q hSP                  . ^R	4      \	        VSV.4      8X  g   Q h\        \        VVV3R
 l4       \        \        VVV3R l4       \        \        VVV3R l4       \        \        VV3R l4       \        \        VVV3R l4       \        4       w   poo\        RSSSSP                  \        4      4       \        4       w   poo\        \        VVV3R l4       \        \        VVVV3R l4       R# )zq u q_J u_Jzq0:4_J u0:4_Jz	q0:4 u0:4r7   )offsetNuT)number_singleqc                  ,   < S P                  SS.^4      # )   _fill_coordinate_listr7   q0q1s   r0   r9   ,test_coordinate_generation.<locals>.<lambda>F       q66BxCr2   c                  0   < S P                  SSR .^R4      # )Nr?   rD   )r7   u0u1s   r0   r9   rI   G   s    q66B~q#Nr2   c                  ,   < S P                  SS.^4      # )   rD   rF   s   r0   r9   rI   H   rJ   r2   c                  >   < S P                  S\        R 4      .^4      # )rH   )rE   r   )r7   rG   s   r0   r9   rI   J   s    a55r74=6I1Mr2   c                  :   < S P                  SS,           S.^4      # )   rD   rF   s   r0   r9   rI   K   s    a55rBwmQGr2   c                  &   < \        R SS SRR.4      # )r7   Nr   )r'   r$   q1js   r0   r9   rI   Q   s    ~c1a#tT9JKr2   c                  (   < \        R SS SSS.R7      # )r7   )speedsrT   )r'   r$   rL   rM   s   r0   r9   rI   R   s    ~c1aRMr2   )
r   r1   r   rE   r
   r   
ValueErrorr;   difft)rA   r?   qjujq0jq2jq3ju0ju1ju2ju3jq2q3u2u3_r'   r7   r$   rG   rH   rU   rL   rM   s                   @@@@@@@@r0   test_coordinate_generationri   &   s[   !-0LA"-;O-L*Cc#%3K%@"BBBB!JAq!QaA""1a(FA3K777""A3*faSk999""A3!"4sCCC""4+vrd|;;;""D61-===""Bd#3Q76	S#<    ""4+vsCo/FFFF""2q)VS#sO-DDDD""2q"3vsCo7NNNN""Bb>1Q"?6	SD   ""41"5
9KKKK""4C0FB4L@@@""2q#.&#C2IIII""4$"?63%=PPP""2q#q$763%=HHH""2q#.&#sC2IIII
:CD
:NO
:CD
9MN
9GH!JAq!QS!QBGGAJ'!JAq!Q
:KL
:MNr2   c                  b   \        R 4      p \        R4      p\        R4      w  r#\        R4      w  rE\        RW4      pVP                  R8X  g   Q hVP
                  V 8X  g   Q hVP                  V8X  g   Q hVP                  \        V.4      8X  g   Q hVP                  \        V.4      8X  g   Q hVP                  \        WTP                  \        4      ,
          .4      8X  g   Q hVP                  V P                  P                  8X  g   Q hVP                   P#                  VP$                  4      \'        ^ 4      8X  g   Q hVP(                  P#                  V P$                  4      \'        ^ 4      8X  g   Q hVP(                  P#                  VP*                  4      \'        ^ 4      8X  g   Q hVP$                  P#                  V P$                  4      \'        ^ 4      8X  g   Q hVP,                  V P                  8X  g   Q hVP.                  VP                  8X  g   Q hVP1                  4       R8X  g   Q h\        R4      p\        R4      p\3        R4      p	V	P5                  VP                  VP6                  \8        ^,          4       \        R	WxW'P                  P                  ,          W8P                  P6                  ,          VP                  P:                  V	R
7      p
V
P<                  VP                  P:                  8X  g   Q hV
P*                  P#                  VP$                  4      W8P                  P6                  ,          8X  g   Q hV
P>                  P#                  VP$                  4      W'P                  P                  ,          8X  g   Q hV
P>                  P#                  V
P*                  4      \'        ^ 4      8X  g   Q hVP$                  P#                  VP$                  4      V) VP                  P                  ,          W8P                  P6                  ,          ,           8X  g   Q hV
P,                  V	8X  g   Q hV
P.                  VP                  8X  g   Q h\        R4      w  rE\A        R4      w  rrrV P$                  PC                  RVP                  VP6                  ,           4      pVP$                  PC                  RVP6                  VP:                  ,           4      p\        RWWEVWWP:                  R7
      pVP                  VP:                  8X  g   Q hVP(                  PE                  V4      ^ 8X  g   Q hVP(                  V8X  g   Q hVP                   V8X  g   Q hVP                   P#                  V P$                  4      VP                  VP6                  ,           8X  g   Q hVP(                  P#                  VP$                  4      VP6                  VP:                  ,           8X  g   Q hVP$                  P#                  V P$                  4      VP                  VP6                  ,           VP6                  ,
          VP:                  ,
          8X  g   Q hVP$                  PE                  V4      PG                  V4      V\I        V4      ,          V\K        V4      ,          ,
          VP                  ,          V) \I        V4      ,          V\K        V4      ,          ,
          VP6                  ,          ,           8X  g   Q hVP,                  V	8X  g   Q hVP.                  V8X  g   Q hR# )r$   r'   l mq_J, u_Jr7   z PinJoint: J  parent: P  child: CP1C1r(   J1parent_pointchild_point
joint_axisparent_interframeq, uTrq   rr   rq   rr   rt   child_interframers   N)&r   r   r   r   namer4   r5   coordinatesr
   rW   kdesrY   rZ   rs   r&   r+   rr   pos_from
masscenterr   rq   _child_pointrt   rw   __str__r   r*   r,   r   z_joint_axis_parent_pointr1   	locatenewvelexpressr	   r   )r$   r'   lmrA   r?   Pjrm   rn   r.   ro   r"   r#   r/   rq   rr   r7   s                    r0   test_pin_jointr   U   s4   #A#A5>DA*%DA	#q	B77c>>99>>88q==>>VQC[(((99s###77fa&&)m_----==AGGII%%%>>""1<<0F1I===??##ALL1VAY>>>??##BOO4q	AAA<<  .&);;;177***!'')))::<====	4B	4B'"DRXXrttR!V,	$Qxxzz\

lrxxzz$(
*B >>RXXZZ'''??##BMM2a((**nDDD$$R]]3q88::~EEE$$R__5BBBMM""2==1BrxxzzMAhhjjL() * )4'''"((***&!DA+D1A!<<)).!##)DL,,((acc	BKaA|("&33	8A <<133>>a A%%%>>\)))==K'''==!!!,,/1339<<<>>""1<<0ACC!##I===<<  .!##)acc/ACC2GGGG<<A&&q)a#a&j1s1v:.E-LBQK!c!f*$P, ., , , ,$&&&%%%r2   c                     \        R 4      w  r\        R4      p\        R4      p\        RV R7      p\        R4      w  rV\	        RW4WVVWP
                  ,          R7      pVP                  V8X  g   Q hVP                  P                  R8X  g   Q hVP                  P                  VP                  4      V) VP
                  ,          8X  g   Q hVP                  VP                  4      \        . RO^ \        V4      \        V4      .^ \        V4      ) \        V4      ..4      8X  g   Q hVP                  P                  VP                  4      V) V,          VP                   ,          8X  g   Q h\        R4      p\        R4      p\        R4      p\        RV R7      p\        R4      w  rV\	        RW4WVVW!VP
                  ,          VP                   R7	      pVP"                  VP                  P                   8X  g   Q hVP                  VP                  4      \        \        V4      \        V4      ^ .\        V4      ) \        V4      ^ .. RO.4      8X  g   Q hVP                  P                  VP                  4      ^ 8X  g   Q hVP                  P                  VP                  4      W,          VP$                  ,          8X  g   Q h\        R	4      w  rrrW3W3W33 UUu/ uF  w  ppVP'                  \(        4      VbK  	  ppp\        R4      \        R4      rC\+        RW4WV4      pVP                  P                  VP                  4      \-        ^4      8X  g   Q hVP                  P                  VP                  4      WWP                  P$                  ,          8X  g   Q hVP                  P                  VP                  4      ^ 8X  g   Q hVP                  P                  VP                  4      WgP                  P$                  ,          8X  g   Q h\        R4      \        R4      rC\        R
4      p\/        RW4WWVWP$                  ,          VP
                  R7
      pVP                  P                  VP                  4      \        \        V	4      ^ \        V	4      .. RO\        V	4      ) ^ \        V	4      ..4      8X  g   Q hVP                  P                  VP                  4      WP$                  ,          WP
                  ,          ,           8X  g   Q hVP                  P                  VP                  4      WP
                  ,          8X  g   Q hVP                  P                  VP                  4      P1                  V4      V) VP
                  ,          W,          VP                   ,          ,
          8X  g   Q h\        R4      \        R4      rC\        R4      p\3        RW4WV.WV.W!VP                   ,          R7	      pVP                  pVP                  P                  VP                  4      \        . RO^ \        V	4      \        V	4      ) .^ \        V	4      \        V	4      ..4      8X  g   Q hVP                  P                  VP                  4      V) VP                   ,          WP
                  ,          ,           WP                   ,          ,           8X  g   Q hVP                  P                  VP                  4      W,          VP
                  ,          WP
                  ,          ,           WP                   ,          ,           8X  g   Q h\        R4      \        R4      rC\        R4      \        R
4      r\5        RW4VW!VP$                  ,          WP
                  ,          R7      pVP                  V4      \-        ^4      8X  g   Q hVP                  P                  VP                  4      WP$                  ,          WP
                  ,          ,
          8X  g   Q hVP                  P                  VP                  4      ^ 8X  g   Q hR# u uppi )m lr'   r$   )massru   r7   rw   rr   	J_P_frame)rt   rw   rr   rs   z	q1:4 u1:4P_frame)rt   rq   rs   C_frame)rt   rw   rq   rr   NrC   r   r   r   r   rC   r   rC   r   )r   r   r   r   r   r,   rw   rt   rx   r|   r{   dcmr
   r   r	   r   r   rs   r+   rY   rZ   r   r   r   xreplacer   r   )r   r   r   r$   r'   rA   r?   r7   r   rH   rd   re   rM   rf   rg   qiui	qdot_to_us                     r0   test_particle_compatibilityr      s   5>DAS!GA1A&!DAaA7]	,A(((##{222<<  .1"wyy.@@@;;q**+vy893q63q67J89CF7CF7K7M 0N N N N <<A//0QBFWYY4FFFFS!GS!GA1A&!DAaAG")799}#II	'A <<1..00000;;q**+vAA7J9<QQ7K7@7B 0C C C C <<A//0A555<<A//0AEGII4EEEE+K8BBB02x"B8.LM.LFBR.LIMC=(3-qsA!'A""1#5#56#a&@@@<<  .!6I6I6K6K2KKKK<<A//0A555<<A//0A8K8K8M8M4MMMMC=(3-qY'GaBBg&'))m			KA""1#5#56&	R!SWyCG8QB*@BB ;C C C C<<  .!ii-"yy.2PPPP<<A//0BNBBB<<A../88C	gii!&799,,. . . C=(3-qY'GCr8Rb%,gii-	IA!!G""1#5#56&As2wR)As2wB+@BB ;C C C C<<  .	
WYYii'"yy.8: : :<<A//0	R))^+b99n<> > > C=(3-q%i0.2KW#qw#*WYY))m	-A ;;w3q6)))<<  .!ii-!ii-2OOOO<<A//0A555O Ns   "ac            
      N   \        R 4      w  r\        R4      p\        4       ;_uu_ 4        \        R4      p\        RWR7      pRRR4       \	        R4      w  rV\        RXXWVWP                  ,          R7       VP                  V8X  g   Q hVP                  P                  R8X  g   Q hVP                  P                  VP                  4      V) VP                  ,          8X  g   Q hVP                  P                  VP                  4      \        . R	O^ \        V4      \        V4      .^ \        V4      ) \        V4      ..4      8X  g   Q hVP                  P                  VP                  4      V) V,          VP                   ,          8X  g   Q hR#   + '       g   i     ELX; i)
r   r'   r$   r   r&   Nru   r7   rr   r   r   )r   r   r    r   r   r   r,   r&   rx   r|   r{   r   r
   r   r	   r   r   )r   r   r   r$   r'   rA   r?   s          r0   test_body_compatibilityr      sJ   5>DAS!G		!	!I1, 
" &!DAS!Q!ii-877g77<<9$$$<<  .1"qss(:::77;;qww69,-s1vs1v+>,-AwA+?+A $B B B B <<AGG$Q444 
"	!	!s   FF$	c            
      z   \        R 4      w  r\        R4      w  r#\        R4      w  rE\        R4      p\        R4      p\        R4      p\        RVR7      p	\        RWtR	7      p
\        R
WR	7      p\	        RWW V) VP
                  ,          V	P                  P                  R7      p\	        RWW1V) VP
                  ,          V
P                  P                  R7      pVP                  V4      \        \        V 4      \        V 4      ) ^ .\        V 4      \        V 4      ^ .. RO.4      8X  g   Q hVP                  V4      \        \        V4      \        V4      ) ^ .\        V4      \        V4      ^ .. RO.4      8X  g   Q h\        VP                  V4      4      \        \        W,           4      \        W,           4      ) ^ .\        W,           4      \        W,           4      ^ .. RO.4      8X  g   Q hVP                  V4      W&P                  ,          8X  g   Q hVP                  V4      W7P                  ,          8X  g   Q hVP                  V4      W&P                  ,          W7P                  ,          ,           8X  g   Q hVP                  \        W P                  \         4      ,
          .4      8X  g   Q hVP                  \        W1P                  \         4      ,
          .4      8X  g   Q hV
P"                  P%                  V4      WR,          VP&                  ,          8X  g   Q hVP"                  P%                  V4      WS,          VP&                  ,          8X  g   Q hVP"                  P%                  V4      WR,          VP&                  ,          WRV,           ,          VP&                  ,          ,           8X  g   Q hR# )zq1 q2zu1 u2r   r"   r#   Br'   r%   r$   r&   r   Rro   )rW   ry   rr   rs   J2Nr   )r   r   r   r   r   r+   r&   r   r   r
   r   r	   r   
ang_vel_inrz   rY   rZ   r|   r   r,   )rH   rd   rM   rf   r   r   r"   r#   r   r'   PartPPartRro   r   s                 r0   test_pin_joint_double_pendulumr      s   G$FBG$FB5>DAsAsAsA#QAc+Ec+E	$ bf
<B	$R bf
@B 558vB#b'15 #BR!4iA B B B B558vB#b'15 #BR!4iA B B B BAEE!H#bg,RWq)I25bg,BGa1P1:)< "= = = =
 <<?b33h&&&<<?b33h&&&<<?b33hcc1111 77fb771:o.////77fb771:o.//// "ad133h..."ad133h..."ad133hGQSS&@@@@r2   c            
         \        R 4      w  rr#p\        R4      w  rVrx\        R4      p	\        R4      p
\        R4      pW4^,          ,
          ^,          pV^,          V^,          ,           p\        RWR7      p\        RWR7      p\        R	V	R
7      p\	        RWWWW*P
                  ,          V	P                  R7      p\	        RWWhWP
                  ,          V
P
                  R7      pV
P                  V	4      \        \        V4      ^ \        V4      ) .. RO\        V4      ^ \        V4      ..4      8X  g   Q hV
P                  V4      \        \        V4      \        V4      ) ^ .\        V4      \        V4      ^ .. RO.4      8X  g   Q hVP                  V	4      \        \        V4      \        V4      ,          \        V4      \        V4      ) \        V4      ,          .\        V4      ) \        V4      ,          \        V4      \        V4      \        V4      ,          .\        V4      ^ \        V4      ..4      8X  g   Q hV
P                  V	4      WyP                  ,          8X  g   Q hV
P                  V4      V) V
P
                  ,          8X  g   Q hV	P                  V4      V) V	P                  ,          WP
                  ,          ,
          8X  g   Q hVP                  \        WuP                  \        4      ,
          .4      8X  g   Q hVP                  \        WP                  \        4      ,
          .4      8X  g   Q hVP                  P!                  VP                  4      W*P
                  ,          8X  g   Q hVP                  P!                  VP                  4      V) V
P
                  ,          8X  g   Q hVP                  P#                  V	4      V^,          V^,          ,
          V,          V
P$                  ,          8X  g   Q hVP                  P#                  V	4      V^,          V^,          ,
          V,          V^,          V^,          ,           V,          ,           V
P$                  ,          8X  g   Q hR# )zmA, mB, lA, lB, hztheta phi omega alphar"   r#   r   rodr   plater   r'   r%   ro   )ry   rW   rr   rs   r   )ry   rW   rq   rs   Nr   r   )r   r   r   r   r   r   r,   r   r
   r   r	   r   rz   rY   rZ   r|   r{   r   r+   )mAmBlAlBhthetaphiomegaalphar"   r#   r   lCr   r   r'   ro   r   s                     r0   test_pin_joint_chaos_pendulumr     s    34BBA-.EFEsAsAsA
1u*	B
Q$1*B
E
,CgB0E#QA	$E f
6B	$!##v!##
7B 558vE
AE
{;( #E
As5z:< = = = = 558vC3s8)Q7 #C#c(A6(* + + + + 558v	S#e*	c#hUCH(<=
c(3u:	s3xS#e*)<=	UQE
#% & & & & <<?eCCi'''<<?ufQSSj(((<<?ufQSSj594444 77fejjm34555577fehhqk123333 <<  0BssF:::>>""5#3#34"qss
BBB >>a QqS2a4Z$6qss$::::"!bd
E'9()!bd
E'9(:;<33'? ? ? ?r2   c                    aaaa \        R 4      w  r\        4       w  r#oo\        R4      \        R4      uoo\        \        VVV3R l4       \        \        VVV3R l4       SP                  W"P                  V 4       SP                  W3P                  V 4       \        \        VVV3R l4       \        \        VVV3R l4       \        4       w  r#oo\        R4      oSP                  V\        ^,          \        \        ^,          3R4       \        RSSWVP                  SP                  ) SSP                  R	7	       \        VP                  V4      4      \        R\        ^4      \!        V 4      ,          ^,          \        ^4      ) \#        V 4      ,          ^,          .\        ^4      ^,          \        ^4      ^\#        V 4      ,          \!        V 4      ,           ,          ^,          \        ^4      \#        V 4      ) ^\!        V 4      ,          ,           ,          ^,          .\        ^4      ^,          \        ^4      R\#        V 4      ,          \!        V 4      ,           ,          ^,          \        ^4      ) \#        V 4      ^\!        V 4      ,          ,           ,          ^,          ..4      ,
          \%        ^4      8X  g   Q hVP'                  V4      VSP                  ,          8X  g   Q hSP(                  P+                  SP(                  4      VP                  VP                  ,           8X  g   Q hSP(                  P-                  V4      WP                  ,          8X  g   Q hSP(                  P-                  S4      \/        ^ 4      8X  g   Q hSP(                  P-                  S4      WP                  ,          8X  g   Q h\        4       w  r#oo\        R4      oSP                  V^\        ,          ^,          \        ) \        ^,          3R4       \        RSSWVP                  ) SP                  SSP                  SP                  ,           R
7	       \        VP                  V4      4      \        \        ^4      ) \#        V 4      ,          ^,          \        ^4      ) \!        V 4      ^,
          ,          ^,          \!        V 4      ^,          ,
          \1        ^4      ^,          ,
          \        ^4      \!        V 4      ^,           ,          ^,          \!        V 4      ^,          ,
          \1        ^4      ^,          ,           .\!        V 4      \        ^4      \        ^4      ,           \#        V 4      ) ,          ^,          \        ^4      ) \        ^4      ,           \#        V 4      ,          ^,          .\        ^4      \#        V 4      ,          ^,          \        ^4      \!        V 4      ^,           ,          ^,          \!        V 4      ^,          ,           \1        ^4      ^,          ,
          \        ^4      ^\!        V 4      ,
          ,          ^,          \!        V 4      ^,          ,           \1        ^4      ^,          ,           ..4      8X  g   Q hVP'                  V4      \        ^4      V,          ^,          VP                  ,          \        ^4      V,          ^,          VP                  ,          ,           8X  g   Q hSP(                  P+                  SP(                  4      VP                  ) VP                  ,
          8X  g   Q hSP(                  P-                  V4      P                  4       \        ^4      ) \        ^4      ,
          V,          ^,          VP                  ,          \        ^4      ) \        ^4      ,           V,          ^,          VP                  ,          ,           8X  g   Q hSP(                  P-                  S4      \/        ^ 4      8X  g   Q h\        4       w  r#oo\        R4      \        R4      uooSP                  V\        ) ^,          \        \        ^,          3R4       SP                  V^\        ,          ^,          \        ) \        ^,          3R4       \        RSSWVP                  VP                  ,
          SP                  ) SSSP                  SP                  ,           R7
       \        VP                  V4      4      \        \!        V 4      \        ^4      \        ^4      ,           \#        V 4      ) ,          ^,          \        ^4      ) \        ^4      ,           \#        V 4      ,          ^,          .\        ^4      ) \#        V 4      ,          ^,          \        ^4      ) \!        V 4      ^,           ,          ^,          \!        V 4      ^,          ,
          \1        ^4      ^,          ,           \        ^4      \!        V 4      ^,
          ,          ^,          \!        V 4      ^,          ,
          \1        ^4      ^,          ,
          .\        ^4      \#        V 4      ,          ^,          \        ^4      \!        V 4      ^,
          ,          ^,          \!        V 4      ^,          ,           \1        ^4      ^,          ,           \        ^4      ) \!        V 4      ^,           ,          ^,          \!        V 4      ^,          ,           \1        ^4      ^,          ,
          ..4      8X  g   Q hVP'                  V4      \        ^4      V,          ^,          SP                  ,          \        ^4      V,          ^,          SP                  ,          ,           8X  g   Q hSP(                  P+                  SP(                  4      VP                  VP                  ,
          VP                  ,           8X  g   Q h\        ^4      ) \        ^4      ,           V,          ^,          VP                  ,          pSP(                  P-                  V4      P                  4       V8X  g   Q hSP(                  P-                  S4      P                  4       V8X  g   Q hSP(                  P-                  S4      \/        ^ 4      8X  g   Q hR# )ru   r.   r/   c                  "   < \        R SS SR7      # r7   rt   r   r'   r$   r.   s   r0   r9   +test_pin_joint_interframe.<locals>.<lambda>D      xQTJr2   c                  "   < \        R SS SR7      # r7   rw   r   r'   r/   r$   s   r0   r9   r   E      xQDIr2   c                  "   < \        R SS SR7      # r   r   r   s   r0   r9   r   I  r   r2   c                  "   < \        R SS SR7      # r   r   r   s   r0   r9   r   J  r   r2   xyzr7   )rq   rr   rt   rs   )rq   rr   rw   rs   rv   Ng      )r   r1   r   r   rX   r*   r   orient_body_fixedr   r   r+   r,   r   r   r
   r   r   r	   r   r   r|   r{   r   r   r   )	rA   r?   r"   r#   N_v_Cr'   r/   r$   r.   s	        @@@@r0   test_pin_joint_interframer   ?  s	   &!DA!JA!Q')?JD$
:JK
:IJQQQQ
:JK
:IJ!JA!Q&!D1rAvr262E:S!Q133QSSD#8AEE!H	a3q6!A%Qx#a&'81'<=	a1d1gSVc!f!459	aSVGa#a&j(	)A	-	/	a1d1gc!fs1v!56:
q'SVa#a&j(	)A	-	/	(0 !1 1
 5:!H= = = <<?a$&&j(((<<  .!##);;;<<A!cc')))<<D!VAY...<<D!QW,,,!JA!Q&!D1q2vzB3Q7?S!QACC4QSS"qssQSSy:AEE!H
q'CF	Q	
q'SVaZ	 1	$s1vz	1AaD1H	<	aCFQJ	!	#c!fqj	01Q4!8	;	= 
Q$q'DG#Aw.27(T!W
A	&	*	,	a3q6	A		aCFQJ	!	#c!fqj	01Q4!8	;	aAAJ	!	#c!fqj	01Q4!8	;	=)> "? ? ? ? <<?d1gkAo3d1gkAo6KKKKK<<  .ACC%!##+===<<A'')	a47a. "#.$&'cc.*Qx$q'!Q-')*-+-.SS-1.1 1 1 1 <<D!VAY...!JA!Q')?JD$1sQwBF3U;1q2vzB3Q7?S!Q1339133$#d) AEE!H	Q$q'DG#Aw.27(T!W
A	&	*	,
q'CF	Q	
q'SVaZ	 1	$s1vz	1AaD1H	<	aCFQJ	!	#c!fqj	01Q4!8	;	= 
a3q6	A		aCFQJ	!	#c!fqj	01Q4!8	;
q'SVaZ	 1	$s1vz	1AaD1H	<	>)? "@ @ @ @ <<?d1gkAo6	:::VV:    <<  .!##)acc/AAA1gXQ1$q(133.E<<A'')U222<<D!**,555<<D!VAY...r2   c                  <  aaaaaa \        R 4      w  op \        R4      w  opoooo\        RSSSV SSSP                  R7      pVP                  SP                  8X  g   Q hSP                  V4      \        \        S4      ^ \        S4      .. RO\        S4      ^ \        S4      ) ..4      8X  g   Q h\        R4      w  opoooo\        RSSSV SSSP                  R7      pVP                  SP                  8X  g   Q hSP                  V4      \        \        S4      ) ^ \        S4      .. RO\        S4      ^ \        S4      ..4      8X  g   Q h\        4       w  opoo\        RSSSV SP                  SP                  ) SP                  R7      pVP                  SP                  8X  g   Q hSP                  V4      \        \        S4      ) \        S4      ) ^ .\        S4      ) \        S4      ^ .. RO.4      8X  g   Q h\        4       w  opoo\        RSSSV SP                  SP                  ) SP                  R7      pVP                  SP                  8X  g   Q hSP                  V4      \        . RO^ \        S4      \        S4      .^ \        S4      \        S4      ) ..4      8X  g   Q h\        R4      w  opoooo\        \        VVVV3R l4       \        \        VV3R l4       \        \        VV3R l4       \        \        VVVV3R l4       \        \        VVVV3R l4       \        R4      w  opoooo\        RSSSSSP                  SP                  ,           R7       \        \        VVVV3R	 l4       \        \        VVVV3R
 l4       R# )ru   Tr7   rt   rw   rs   c            	         < \        R SS \        S4      SP                  ,          \        S4      SP                  ,          ,           R7      # r7   )rs   )r   r   r+   r	   r,   r'   r"   r$   rA   s   r0   r9   +test_pin_joint_joint_axis.<locals>.<lambda>  s1    xQ36q6ACC<#a&133,3N Pr2   c                  6   < \        R SS S P                  R7      # r   r   r+   r'   r$   s   r0   r9   r     s    xQaccBr2   c                  X   < \        R SS SP                  S P                  ,           R7      # r   r   r+   r,   r   s   r0   r9   r     s    xQaccACCiHr2   c            	      \   < \        R SS SSSP                  S P                  ,           R7      # r7   r   r   r'   r/   r$   r.   s   r0   r9   r     s$    xQTD66ACC< !r2   c            	      \   < \        R SS SSSP                  SP                  ,           R7      # r   r   r   s   r0   r9   r     s$    xQTD33< !r2   c            
      8   < \        R SS SS\        ^ 4      R7      # r   )r   r   r   s   r0   r9   r     s    hQTD!9r2   c            	      \   < \        R SS SSSP                  SP                  ,           R7      # r   )r   r,   r   s   r0   r9   r     s$    hQTD33<!r2   Nr   r   )r   r   r   r   r   r   )r   r1   r   r,   rs   r   r
   r	   r   r   r+   r   rX   	Exception)	r?   r#   pinr'   r/   r"   r$   r.   rA   s	      @@@@@@r0   test_pin_joint_joint_axisr     sA   &!DAq+D1Aq!Qd
31ad$(QSS:C>>QSS   558vA3q62J #ACF735 6 6 6 6 ,D1Aq!Qd
31ad$(TVV=C>>TVV###558vQCF3Z #A3q624 5 5 5 5  !JAq!Q
31aacc%&SSDQSS:C>>QSS   558vQ#a&!4AwA6J)+ , , , ,!JAq!Q
31aacc%&SSDQSS:C>>QSS   558vzAs1vs1v+> !3q6CF735 6 6 6 6 ,D1Aq!Qd
: P Q :BC
:HI
: ! " : ! " ,D1Aq!QdS!Q$& 9   9 ! "r2   c                  9   \        R 4      w  r\        4       w  r#rE\        RWEVP                  ) R7       VP                  ) P	                  VP                  4      ^ 8X  g   Q hVP                  P                  V4      ) VP                  8X  g   Q hVP                  V4      \        . R	O^ \        V 4      ) \        V 4      ) .^ \        V 4      ) \        V 4      ..4      8X  g   Q hVP                  V4      WP                  ,          8X  g   Q hVP                  V4      P                  4       \        V^,          4      8X  g   Q hVP                  P                  VP                  4      ^ 8X  g   Q hVP                  P                  VP                  4      P                  V4      P                  4       ^ 8X  g   Q hVP                  P!                  V4      ^ 8X  g   Q h\        4       w  r#rE\        RWEVP"                  VP                  R7       VP"                  P	                  VP                  4      ^ 8X  g   Q hVP"                  P                  V4      VP                  8X  g   Q hVP                  V4      \        ^ \        V 4      ) \        V 4      ) .. R
O^ \        V 4      ) \        V 4      ..4      8X  g   Q hVP                  V4      WP                  ,          8X  g   Q hVP                  V4      P                  V4      WP"                  ,          8X  g   Q hVP                  V4      P                  4       \        V^,          4      8X  g   Q hVP                  V4      P%                  VP"                  4      ^ 8X  g   Q hVP                  P!                  V4      WP&                  ,          8X  g   Q hVP                  P                  VP                  4      VP                  ) 8X  g   Q hVP                  P                  VP                  4      P                  V4      P                  4       \        V 4      VP"                  ,          \        V 4      VP&                  ,          ,           8X  g   Q hVP                  P!                  V4      P	                  VP                  4      \(        ^,          8X  g   Q h\        4       w  r#rE\        RWEVP"                  VP                  R7       VP"                  P	                  VP                  4      ^ 8X  g   Q hVP"                  P                  V4      VP                  8X  g   Q hVP                  V4      \        . RO\        V 4      ) ^ \        V 4      .\        V 4      ^ \        V 4      ..4      8X  g   Q hVP                  V4      WP"                  ,          8X  g   Q hVP                  V4      P                  V4      WP                  ,          8X  g   Q hVP                  V4      P                  4       \        V^,          4      8X  g   Q hVP                  V4      P	                  VP                  4      pVP+                  V^/4      ^ 8X  g   Q hVP                  P!                  V4      ^ 8X  g   Q hVP                  P                  VP                  4      VP                  8X  g   Q h\        4       w  r#rE\        RWEVP                  VP                  VP                  VP"                  ,           R7       \-        VP                  P	                  VP                  VP"                  ,           4      4      ^ 8X  g   Q hVP                  VP"                  ,           P                  V4      P                  4       \        ^4      VP                  ,          8X  g   Q h\        VP                  V4      4      \        \        ^4      ^,          \        ^4      ) \        V 4      ,          ^,          \        ^4      ) \        V 4      ,          ^,          .\        ^4      ^,          \        ^4      \        V 4      ,          ^,          \        ^4      \        V 4      ,          ^,          .^ \        V 4      ) \        V 4      ..4      8X  g   Q hVP                  V4      WP                  ,          8X  g   Q hVP                  V4      P                  V4      P                  4       WP                  ,          WP"                  ,          ,           \        ^4      ,          8X  g   Q hVP                  V4      P                  4       \        V^,          4      8X  g   Q hVP                  V4      P	                  VP                  VP"                  ,           4      pVP+                  V^/4      ^ 8X  g   Q hVP                  P!                  V4      P                  4       WP&                  ,          \        ^4      ,          8X  g   Q hVP                  P                  VP                  4      VP                  VP                  ,
          8X  g   Q hVP                  P                  VP                  4      P                  V4      P                  4       ^\        ^4      ^,          ,
          VP                  ,          \        ^4      \        V 4      ,          ^,          VP"                  ,          ,           \        ^4      \        V 4      ,          ^,          VP&                  ,          ,           8X  g   Q hVP                  P!                  V4      P                  V4      P                  4       \        ^4      ) V,          \        V 4      ,          ^,          VP"                  ,          \        ^4      V,          \        V 4      ,          ^,          VP&                  ,          ,           8X  g   Q hVP                  P!                  V4      P	                  VP                  4      \(        ^,          8X  g   Q h\        4       w  r#rE\        RWEVP                  VP                  VP                  VP"                  ,           VP&                  ,
          R7       \-        VP                  P	                  VP                  VP"                  ,           VP&                  ,
          4      4      ^ 8X  g   Q hVP                  VP"                  ,           VP&                  ,
          P                  V4      P                  4       \        ^4      VP                  ,          8X  g   Q h\        VP                  V4      4      \        \        ^4      ^,          \        ^4      ) \        V \(        ^,          ,           4      ,          ^,          \        ^4      \        V \(        ^,          ,           4      ,          ^,          .\        ^4      ^,          \        ^4      \        V \(        ^,          ,           4      ,          ^,          \        ^4      \        V \(        ^,          ,           4      ,          ^,          .\        ^4      ) ^,          \        ^4      \        V ^\(        ,          ^,          ,           4      ,          ^,          \        ^4      \        V ^\(        ,          ^,          ,           4      ,          ^,          ..4      8X  g   Q hVP                  V4      WP                  ,          8X  g   Q hVP                  V4      P                  V4      P                  4       WP                  ,          WP"                  ,          ,           WP&                  ,          ,
          \        ^4      ,          8X  g   Q hVP                  V4      P                  4       \        V^,          4      8X  g   Q hVP                  V4      P	                  VP                  VP"                  ,           VP&                  ,
          4      pVP+                  V^/4      P                  4       ^ 8X  g   Q hVP                  P!                  V4      P                  4       WP"                  ,          WP&                  ,          ,           \        ^4      ,          8X  g   Q hVP                  P                  VP                  4      VP                  VP                  ,
          8X  g   Q hVP                  P                  VP                  4      P                  V4      P                  4       ^\        ^4      ^,          ,
          VP                  ,          \        ^4      \        V \(        ^,          ,           4      ,          ^,          VP"                  ,          ,           \        ^4      \        V \(        ^,          ,           4      ,          ^,          VP&                  ,          ,
          8X  g   Q hVP                  P!                  V4      P                  V4      P                  4       \        ^4      V,          \        V \(        ^,          ,           4      ,          ^,          VP"                  ,          \        ^4      V,          \        V \(        ^,          ,           4      ,          ^,          VP&                  ,          ,           8X  g   Q hVP                  P!                  V4      P	                  VP                  4      \(        ^,          8X  g   Q h\        4       w  r#rE\/        R4      w  rx\        RWEWrP                  ,          WP                  ,          VP                  VP"                  ,           VP&                  ,
          VP                  VP"                  ,
          VP&                  ,           R7       VP                  VP"                  ,
          VP&                  ,           P	                  VP                  VP"                  ,           VP&                  ,
          4      p\-        V4      ^ 8X  g   Q hVP                  VP"                  ,
          VP&                  ,           P                  V4      P                  4       R\        V 4      ,          ^,          \1        ^4      ^,          ,
          VP                  ,          \1        ^4      ^,          ^\        V \(        ^,          ,           4      ,          ^,          ,
          VP"                  ,          ,           ^\        V \(        ^,          ,           4      ,          ^,          \1        ^4      ^,          ,
          VP&                  ,          ,           8X  g   Q h\        VP                  V4      4      \        \1        ^4      ^,          ^\        V 4      ,          ^,          ,
          R\        V \(        ^,          ,           4      ,          ^,          \1        ^4      ^,          ,
          ^\        V \(        ^,          ,           4      ,          ^,          \1        ^4      ^,          ,           .^\        V \(        ^,          ,           4      ,          ^,          \1        ^4      ^,          ,           ^\        V 4      ,          ^,          \1        ^4      ^,          ,
          ^\        V \(        ^,          ,           4      ,          ^,          \1        ^4      ^,          ,           .R\        V \(        ^,          ,           4      ,          ^,          \1        ^4      ^,          ,
          ^\        V \(        ^,          ,           4      ,          ^,          \1        ^4      ^,          ,           ^\        V 4      ,          ^,          \1        ^4      ^,          ,
          ..4      8X  g   Q hVP                  V4      WP                  ,          WP"                  ,          ,
          WP&                  ,          ,           \        ^4      ,          ,
          P                  4       '       g   Q hVP                  V4      P                  V4      P                  4       WP                  ,          WP"                  ,          ,           WP&                  ,          ,
          \        ^4      ,          8X  g   Q hVP                  V4      P                  4       \        V^,          4      8X  g   Q hVP                  V4      P	                  VP                  VP"                  ,           VP&                  ,
          4      pVP+                  V^/4      P                  4       ^ 8X  g   Q hVP                  P!                  V4      P                  4       \        ^4      V,          V,          ^,          VP"                  ,          \        ^4      V,          V,          ^,          VP&                  ,          ,           8X  g   Q hVP                  P                  VP                  4      WrP                  ,          WP                  ,          ,
          8X  g   Q hVP                  P                  VP                  4      P                  V4      P                  4       Wx^\        V 4      ,          ^,
          ,          ^,          ,           VP                  ,          V^\        V \(        ^,          ,           4      ,          ^,           ,          ^,          VP"                  ,          ,           V^\        V \(        ^,          ,           4      ,          ^,           ,          ^,          VP&                  ,          ,
          8X  g   Q hVP                  P!                  V4      P                  V4      P                  4       RV,          V,          \        V 4      ,          ^,          VP                  ,          ^V,          V,          \        V \(        ^,          ,           4      ,          ^,          VP"                  ,          ,           ^V,          V,          \        V \(        ^,          ,           4      ,          ^,          VP&                  ,          ,           8X  g   Q hVP                  P!                  V4      P3                  VP                  VP"                  ,
          VP&                  ,           4      P                  4       ^ 8X  g   Q hR# )rl   r7   r   r   rt   rq   rq   rr   rw   m nrq   rr   rw   rt   Nr   r   r   r   )r   r1   r   r+   angle_betweenr   r   r
   r   r	   r   	magnituder   r|   r{   r   r   r,   crossr   r   r   r   r   r   dot)	rA   r?   r"   r#   r$   r'   angler   ns	            r0   test_pin_joint_arbitrary_axisr     s   *%DA  !JA!S!!##.SSD$)))CCKKN?acc!!!558vz !CF7SVG4 !CF7CF35 6 6 6 6 <<?ae###<<?$$&$q!t*444<<  .!333<<  .66q9BBDIII<<A!###  !JA!S!!##>33QSS!Q&&&33;;q>QSS   558vCF7SVG4( !CF7CF35 6 6 6 6 <<?ae###<<?""1%SS000<<?$$&$q!t*444<<?  %***<<A!CC%'''<<  .133$666LL!!!,,/77:CCEF133JQ#$ % $<<A,,QSS1RT999  !JA!S!!##ACC@33QSS!Q&&&33;;q>QSS   558vy!$QCF3 #A3q624 5 5 5 5 <<?ae###<<?""1%33...<<?$$&$q!t*444LLO))!##.E>>1a&!Q&&&<<A!###<<  .!##555  !JA!S!QSSaccccACCi)acc''acc	23q888CC!##Iq!**,Q;;;AEE!H	aT!WHSVOA%QxAq'89	aDGCFN1$d1gc!fnQ&67	
SVGSV) "    <<?ae###LLO##A&//1ssUQssU]DG#$ % $<<?$$&$q!t*444LLO))!##)4E>>1a&!Q&&&<<A'')a##gtAw->>>><<  .!##);;;LL!!!,,/77:CCEa]ACC$q'#a&."2133"66GCFN1QSS !! " ! LLQ''*335!WHQJs1va#d1gaiA&6q&8&<<= > =<<A,,QSS1RT999!JA!S!QSSaccccACCi!##o/acc''acc	ACC89Q>>>CC!##IO$$Q'002d1gacckAAAAEE!H	aT!WHSRT]*1,	aQAX	q	 	"	aDGCBrE	N*1,	aQBY		!	#
q'!T!WSQrT"W--a/	aQ2b[!	!!	#	%)& "' ' ' ' <<?ae###<<?""1%..0QssUQssU]56ssU6;<@G5D D D D<<?$$&$q!t*444LLO))!##)ACC-8E>>1a&!**,111<<A'')aeaemT!W-DDDD<<  .!##);;;LL!!!,,/77:CCEa]ACC$q'#a"Q$h-"7"9!##"==GCBqDM!!#ACC'(( ) ( LLQ''*335GAIc!bd(m#A%acc)GAIc!bd(m#A%acc)** + * <<A,,QSS1RT999!JA!5>DAS!QW!cc'ccACCi!##o ssQSSy1330 SS133Y_++ACC!##IO<Ee!!!SSWQSS[!!!$--/AYq[1Q46!133&!A$q&1SRT]?13D*Dacc)IIs1r!t8}_Q1a',-- . - AEE!H	
1a!CF(1*	bQAX.q01Q469	
3q2a4x=	QqT!V	#	%	
3q2a4x=	QqT!V	#Qs1vXaZ!A$q&%8	
3q2a4x=	QqT!V	#	%	CBqDM	!	ad1f	$aA1Hoa&7!A$q&&@	
3q6!ad1f		) "        LLOququ}qu4d1g==GGIIII<<?""1%..0QssUQssU]56ssU6;<@G5D D D D<<?$$&$q!t*444LLO))!##acc'!##+6E>>1a&!**,111LLQ((*GAIaKM!##Q	!Aacc 112 3 2<<  .!CC%!CC%-???LL!!!,,/77:CCEAc!fHqL!!##QSS(1aA1Ho/ ,,, !CBqDM/A-.q0455 6 5 LLQ''*335E!GCFN1QSS 1Q3q5QAX#6q#8#<<aCE#a"Q$h-!!##%&& ' & <<A""1339qss?3<<>!CCCr2   c                     \        4       w  rr#\        P                  ! W"P                  ) VP                  4      pVP                  VP                  8X  g   Q h\        P                  ! W"P
                  ) VP
                  4      pVP                  VP                  8X  g   Q h\        P                  ! W"P                  ) VP                  4      pVP
                  VP
                  8X  g   Q h\        P                  ! W"P                  ) VP
                  ,
          VP                  VP
                  ,           4      pVP                  VP                  8X  g   Q h\        P                  ! W"P
                  ) VP                  ,
          VP
                  VP                  ,           4      pVP                  VP                  8X  g   Q h\        P                  ! W"P                  ) VP                  ,
          VP                  VP                  ,           4      pVP
                  VP
                  8X  g   Q h\        P                  ! W"P                  ) VP
                  ,
          VP                  ,
          VP                  VP
                  ,           VP                  ,           4      pVP
                  VP                  ,
          VP
                  VP                  ,
          8X  g   Q hR # N)r1   r   _create_aligned_interframer+   r   r,   )r"   r#   r$   r'   fs        r0   test_create_aligned_frame_pir   R  s   !JA!((SSD!##6A33!##::((SSD!##6A33!##::((SSD!##6A33!##::((SSD133Jacc	BA33!##::((SSD133Jacc	BA33!##::((SSD133Jacc	BA33!##::((SSD133J,<accACCi!##oNA339acc	!!!r2   c                    aaaa	 \        R 4      w  o	p \        R4      w  opoor#\        RSSS	WVR7      pVP                  VP                  8X  g   Q h\        ^ \        S	4      \        S	4      .^ \        S	4      ) \        S	4      .. RO.4      p\        R4      w  opoor#\        RSSS	WVSP                  R7       SP                  V4      V8X  g   Q h\        R4      w  opoor#\        RSSS	WVVP                  ) R7       SP                  V4      V8X  g   Q h\        R4      w  opoor#\        \        VVVV	3R l4       R# )zq uTr7   )rt   rw   r   c                  D   < \        R SS SSP                  ,          R7      # r   )r   r   r   s   r0   r9   %test_pin_joint_axis.<locals>.<lambda>x  s    xQa!##gFr2   Nr   )r   r1   r   rs   r+   r
   r	   r   r   r   r   rX   )
r?   r#   r.   r/   r7   N_R_Ar'   r"   r$   rA   s
         @@@@r0   test_pin_joint_axisr   d  s7   % DAq+D1Aq!QaAq4PA<<466!!!QAA'QQ(  E ,D1Aq!QS!Q1t558u+D1Aq!QS!Q1t!558u+D1Aq!Q
:FGr2   c                  6  aaaaa \        4       w  op oo\        R SSSP                  SP                  ,           R7      pVP                  P
                  R8X  g   Q hVP                  P                  SP                  4      SP                  SP                  ,           8X  g   Q hVP                  SP                  8X  g   Q h\        4       w  op ooSP                  P                  RSP                  SP                  ,           4      p\        R SSVSP                  R7      pVP                  V8X  g   Q hVP                  SP                  8X  g   Q h\        4       w  op oo\        \        VVV3R l4       \        R4      o\        4       w  op oo\        \        VVVV3R l4       \        4       w  op ooSP                  P                  RSV P                  ,          4      o\        \        VVV3R l4       \        R4      o\        \        VVV3R	 l4       R
# )r7   rq   	J_P_jointp)rq   rr   c                  T   < \        R SS SP                  P                  S4      R7      # r7   r   )r   r+   	to_matrix)r'   r"   r$   s   r0   r9   'test_locate_joint_pos.<locals>.<lambda>  s    8CAACCMM!4DEr2   rA   c                  D   < \        R SS SSP                  ,          R7      # r   r   r   s   r0   r9   r     s    xQACCHr2   c                  "   < \        R SS SR7      # r7   r   r   r'   r$   rr   s   r0   r9   r         xQ{Kr2   c                  "   < \        R SS SR7      # r   r   r  s   r0   r9   r     r  r2   N)r1   r   r,   r   rq   rx   r{   r|   rr   r   r   r;   r   rX   r   )r#   jointrq   r'   r"   r$   rr   rA   s      @@@@@r0   test_locate_joint_posr  {  s   !JAq!QS!QQSS133Y7E""k111&&q||4acc	AAA,,,!JAq!Q<<))#qssQSSy9LS!Q\!"/E---,,,!JAq!Q
9EG 	sA!JAq!Q
:HI!JAq!Q,,((a!##g6K
:KL*K
:KLr2   c                  h  aaaa \        4       w  roo\        R 4      oSP                  W P                  ^4       \	        RSSSR7      pVP
                  S8X  g   Q hVP
                  P                  V 4      ^ 8X  g   Q hVP                  V8X  g   Q h\        R4      p\        4       w  roo\        R 4      oSP                  W P                  V4       \        \        VVV3R l4       \        4       w  roo\        R 4      oSP                  W P                  ^4       \        \        VVV3R l4       R# )	int_framer7   r   rA   c                  "   < \        R SS SR7      # r   r   )r'   r$   rt   s   r0   r9   )test_locate_joint_frame.<locals>.<lambda>  s    8CA9JKr2   c                  "   < \        R SS SR7      # r   r   )r'   r$   rw   s   r0   r9   r	    s    8CA8HIr2   N)r1   r   r*   r   r   rt   r   rw   r   r   rX   )r"   r#   r  rA   r'   r$   rw   rt   s       @@@@r0   test_locate_joint_framer    s   !JA!Q&{3!!!SS!,S!Q2CDE""&7777""--a0A555!!Q&&&sA!JA!Q&{3!!!SS!,
:KM  !JA!Q%k2  CC+
:IKr2   c            
      n   \        4       w   rp\        R 4      w  r4\        RW4      pVP                  R8X  g   Q hVP                  V8X  g   Q hVP
                  V8X  g   Q hVP                  \        V.4      8X  g   Q hVP                  \        V.4      8X  g   Q hVP                  \        WCP                  \        4      ,
          .4      8X  g   Q hVP                  VP                  P                  8X  g   Q hVP                  P!                  VP"                  4      \%        ^ 4      8X  g   Q hVP&                  P!                  VP"                  4      \%        ^ 4      8X  g   Q hVP&                  P!                  VP                  4      V) VP                  P                  ,          8X  g   Q hVP"                  P!                  VP"                  4      V) VP                  P                  ,          8X  g   Q hVP"                  P)                  VP                  4      WAP                  P                  ,          8X  g   Q hVP                  P+                  VP                  4      ^ 8X  g   Q hVP                  P+                  VP                  4      ^ 8X  g   Q hVP,                  ! 4       R8X  g   Q h\        4       w  rgr\/        R4      w  r\1        R4      p
V
P3                  VP                  VP4                  \6        ^,          4       \        RWWP                  P                  ,          WP                  P4                  ,          VP                  P8                  V
R7      pVP                  VP                  P8                  8X  g   Q hVP                  P!                  VP"                  4      WP                  P4                  ,          8X  g   Q hVP&                  P!                  VP"                  4      WP                  P                  ,          8X  g   Q hVP&                  P!                  VP                  4      V) VP                  P8                  ,          8X  g   Q hVP"                  P!                  VP"                  4      V) VP                  ,          W6P8                  ,          ,
          WP4                  ,          ,           8X  g   Q hVP"                  P)                  VP                  4      WAP                  P8                  ,          8X  g   Q hVP"                  P)                  V
4      \%        ^ 4      8X  g   Q hVP                  P+                  VP                  4      ^ 8X  g   Q hVP                  P+                  VP                  4      ^ 8X  g   Q h\        4       w   rp\1        R4      p
V
P3                  VP                  VP4                  \6        ^,          4       \        RWWP                  P8                  ,          WP                  P                  ,          VP                  P8                  V
R7      pVP                  VP                  P8                  8X  g   Q hVP                  P!                  VP"                  4      WP                  P                  ,          8X  g   Q hVP&                  P!                  VP"                  4      WP                  P8                  ,          8X  g   Q hVP&                  P!                  VP                  4      V) VP                  P8                  ,          8X  g   Q hVP"                  P!                  VP"                  4      V) V,
          VP                  P8                  ,          WP                  P                  ,          ,           8X  g   Q hVP"                  P)                  VP                  4      WAP                  P8                  ,          8X  g   Q hVP                  P+                  VP                  4      ^ 8X  g   Q hVP                  P+                  VP                  4      ^ 8X  g   Q hR# )q_S, u_Sr   z&PrismaticJoint: S  parent: P  child: Crk   r(   rp   N)r1   r   r   rx   r4   r5   ry   r
   rW   rz   rY   rZ   rs   r&   r+   rr   r{   r|   r   rq   r   r   r~   r   r   r*   r,   r   r   )rh   r$   r'   rA   r?   r   r"   r#   r   r   r.   s              r0   test_prismatic_jointr    s   !JAqQ*%DAsA!A66S==88q==77a<<==FA3K'''88vqc{"""66VQ]O,,,,<<17799$$$==!!!,,/6!9<<<>>""1<<0F1I===>>""1==1qS17799_DDD<<  .A#		/AAA<<AGG$GGII55577agg&!+++77agg&!+++99;BBBB!JA!5>DA'"DQWWacc26*sAq7799}#$wwyy=QWWYY)-	/A <<17799$$$==!!!,,/1wwyy=@@@>>""1<<0A		MAAA>>""1==1qS17799_DDD<<  .A#)a##g2ECC2OOOO<<AGG$GGII555<<D!VAY...77agg&!+++77agg&!+++!JAqQ'"DQWWacc26*sAq7799}#$wwyy=QWWYY)-	/A <<17799$$$==!!!,,/1wwyy=@@@>>""1<<0A		MAAA>>""1==1qS17799_DDD<<  .A26177992Dq{2RRRR<<AGG$GGII55577agg&!+++77agg&!+++r2   c                  &   \        R 4      w  r\        4       w  r#rE\        RWEVP                  ) R7       VP                  ) P	                  VP                  4      ^ 8X  g   Q hVP                  P                  V4      ) VP                  8X  g   Q hVP                  V4      \        . R	O. R
O. RO.4      8X  g   Q hVP                  P                  VP                  4      WP                  ,          8X  g   Q hVP                  P                  VP                  4      P                  V4      P                  4       V ) VP                  ,          8X  g   Q hVP                  P                  V4      WP                  ,          8X  g   Q hVP                  P                  V4      P                  V4      V) VP                  ,          8X  g   Q hVP                  V4      ^ 8X  g   Q hVP                  V4      ^ 8X  g   Q h\        4       w  r#rE\        RWEVP                  VP                  R7       VP                  P	                  VP                  4      ^ 8X  g   Q hVP                  P                  V4      VP                  8X  g   Q hVP                  V4      \        . R
O. RO. RO.4      8X  g   Q hVP                  P                  V4      WP                  ,          8X  g   Q hVP                  P                  V4      P                  V4      WP                  ,          8X  g   Q hVP                  P                  VP                  4      WP                  ,          VP                  ,
          8X  g   Q hVP                  P                  VP                  4      P                  V4      P                  4       WP                  ,          VP                  ,           8X  g   Q hVP                  V4      ^ 8X  g   Q hVP                  V4      ^ 8X  g   Q h\        4       w  r#rE\        RWEVP                  VP                  R7       VP                  P	                  VP                  4      ^ 8X  g   Q hVP                  P                  V4      VP                  8X  g   Q hVP                  V4      \        . RO. R	O. RO.4      8X  g   Q hVP                  P                  V4      WP                  ,          8X  g   Q hVP                  P                  V4      P                  V4      WP                  ,          8X  g   Q hVP                  P                  VP                  4      VP                  WP                  ,          ,           8X  g   Q hVP                  V4      ^ 8X  g   Q hVP                  V4      ^ 8X  g   Q h\        4       w  r#rE\        RWEVP                  VP                  VP                  VP                  ,           R7       VP                  P	                  VP                  VP                  ,           4      ^ 8X  g   Q hVP                  VP                  ,           P                  V4      \        ^4      VP                  ,          8X  g   Q hVP                  V4      \        \        ^4      ^,          \        ^4      ) ^,          ^ .\        ^4      ^,          \        ^4      ^,          ^ .. RO.4      8X  g   Q hVP                  P                  VP                  4      V ^,           VP                  ,          VP                  ,
          8X  g   Q hVP                  P                  VP                  4      P                  V4      V \        ^4      ^,          ,
          ^,           VP                  ,          \        ^4      ^,          VP                  ,          ,           8X  g   Q hVP                  P                  V4      P                  V4      WP                  VP                  ,           ,          \        ^4      ,          8X  g   Q hVP                  P                  V4      WP                  ,          8X  g   Q hVP                  V4      ^ 8X  g   Q hVP                  V4      ^ 8X  g   Q h\        4       w  r#rE\        RWEVP                  VP                  VP                  VP                  ,           VP                  ,
          R7       VP                  P	                  VP                  VP                  ,           VP                  ,
          4      P                  4       ^ 8X  g   Q hVP                  VP                  ,           VP                  ,
          P                  V4      \        ^4      VP                  ,          ,
          P                  4       ^ 8X  g   Q h\        VP                  V4      4      \        \        ^4      ^,          \        ^4      ) ^,          \        ^4      ^,          .\        ^4      ^,          \        ^4      ^,          \!        ^4      ^,          ,           \!        ^4      ^,          \        ^4      ^,          ,
          .\        ^4      ) ^,          \!        ^4      ^,          \        ^4      ^,          ,
          \        ^4      ^,          \!        ^4      ^,          ,           ..4      8X  g   Q hVP                  P                  VP                  4      V ^,           VP                  ,          VP                  ,
          8X  g   Q hVP                  P                  VP                  4      P                  V4      V \        ^4      ^,          ,
          ^,           VP                  ,          \        ^4      ^,          VP                  ,          ,           \        ^4      ^,          VP                  ,          ,
          ,
          P                  4       ^ 8X  g   Q hVP                  P                  V4      WP                  ,          8X  g   Q hVP                  P                  V4      P                  V4      \        ^4      V,          ^,          VP                  ,          \        ^4      V,          ^,          VP                  ,          ,           \        ^4      V,          ^,          VP                  ,          ,
          ,
          P                  4       '       g   Q hVP                  V4      ^ 8X  g   Q hVP                  V4      ^ 8X  g   Q h\        4       w  r#rE\#        R4      w  rg\        RWEWbP                  ,          WsP                  ,          VP                  VP                  ,           VP                  ,
          VP                  VP                  ,
          VP                  ,           R7       VP                  VP                  ,
          VP                  ,           P	                  VP                  VP                  ,           VP                  ,
          4      P                  4       ^ 8X  g   Q hVP                  VP                  ,           VP                  ,
          P                  V4      VP                  VP                  ,
          VP                  ,           ,
          P                  4       ^ 8X  g   Q h\        VP                  V4      4      \        \!        ^4      ) ^,          \!        ^4      ) ^,          \!        ^4      ^,          .\!        ^4      ^,          \!        ^4      ^,          \!        ^4      ^,          .\!        ^4      ) ^,          \!        ^4      ^,          \!        ^4      ^,          ..4      8X  g   Q hVP                  P                  VP                  4      V\        ^4      V ,          ^,          ,           VP                  ,          \        ^4      V ,          ^,          VP                  ,          ,
          \        ^4      V ,          ^,          VP                  ,          ,           WsP                  ,          ,
          ,
          P                  V4      P                  4       ^ 8X  g   Q hVP                  P                  VP                  4      P                  V4      Wg^,          ,           \        ^4      V ,          ^,          ,           VP                  ,          ^V,          ^,          \        ^4      V ,          ^,          ,
          VP                  ,          ,           RV,          ^,          \        ^4      V ,          ^,          ,           VP                  ,          ,           ,
          P                  4       ^ 8X  g   Q hVP                  P                  V4      P                  V4      \        ^4      V,          ^,          VP                  ,          \        ^4      V,          ^,          VP                  ,          ,
          \        ^4      V,          ^,          VP                  ,          ,           ,
          P                  4       ^ 8X  g   Q hVP                  P                  V4      P                  V4      \        ^4      V,          ^,          VP                  ,          \        ^4      V,          ^,          VP                  ,          ,           \        ^4      V,          ^,          VP                  ,          ,
          ,
          P                  4       ^ 8X  g   Q hVP                  V4      ^ 8X  g   Q hVP                  V4      ^ 8X  g   Q hR# )r  r   r   r   r   r   r   r   Nr   r   r   r   r   r   )r   r1   r   r+   r   r   r   r
   r|   r{   r   r   r   r,   r   r   r   r   )rA   r?   r"   r#   r$   r'   r   r   s           r0   #test_prismatic_joint_arbitrary_axisr    s~   *%DA!JA!3t4SSD$)))CCKKN?acc!!!558vz:yABBBB<<  .!cc'999<<  .66q9BBDQSSPPP<<A!cc')))<<A&&q)aR!##X555<<?a<<?a  !JA!3qssD33QSS!Q&&&33;;q>QSS   558vz9i@AAAA<<A!cc')))<<A&&q)QW444<<  .!CC%!##+===<<  .66q9BBD##PQPSPSSSS<<?a<<?a  !JA!3!##F33QSS!Q&&&33;;q>acc!!!558vy*i@AAAA<<A!cc')))<<A&&q)QW444<<  .!####+===<<?a<<?a  !JA!3133ACC$%CC!##I/33QSS133Y'1,,,CC!##Iq!T!WQSS[000558vQ	DG8A:q9DGAItAwqyRS;TV_`aaaa<<  .1q5!##+2CCCC<<  .66q9a$q'!)ma>OQRQTQT=TW[\]W^_`W`abadadWd=dddd<<A&&q)Q##)_T!W-DDDD<<A!CC%'''<<?a<<?a!JA!3133ACC$%CC!##IO533QSS133Y_-668A===SS133Y_%%a(47133;6@@BaGGGAEE!H$q'!)d1gXaZa)K26q'!)T!WQY1a=OQRSTQUVWQWZ^_`ZabcZcQc1d377(1*ad1ftAwqy>PRVWXRYZ[R[^_`a^bcd^dRd1e)g "h h h h <<  .1q5!##+2CCCCLL!!!,,/77:
d1gai-!
QSS	 4719QSS=	04719QSS=	@BCK8:QRS S S<<A!CC%'''LLQ''*Q	!ACC$q'!)A+acc/)DGAIaKO;=>FhjI I I<<?a<<?a!JA!5>DA31SS5ae$%CC!##IO%&SS133Y_6 CCGACCK&&qss133wqss{3<<>!CCCSSWQSS[!!!$acc	ACC8BBDIIIAEE!H1Q4%'AaD57AaDF)C23A$q&!A$q&!A$q&1I34Q4%'1Q461Q461J)L "M M M M LL!!!,,/	
T!WQYq[!##Q	!ACC/$q'!)A+acc/AAccEIKgajq) ) ) LL!!!,,/77:	
qS4719Q;	#qs1utAwqy{':ACC&??	Aa$q'!)A+	qss"	#$%-XZ15 5 5 LLQ''*Q	!ACC$q'!)A+acc/)DGAIaKO;=>FhjAN N NLLQ''*!WQYq[_tAwqy{133.a1QSS@BCK8:QRS S S<<?a<<?ar2   c                     \        4       w  rr#\        R 4      w  rErg\        RW#4      pVP                  R8X  g   Q hVP                  V8X  g   Q hVP
                  V8X  g   Q hVP                  \        WE.4      8X  g   Q hVP                  \        Wg.4      8X  g   Q hVP                  V8X  g   Q hVP                  V8X  g   Q hVP                  V8X  g   Q hVP                  V8X  g   Q hVP                  \        WdP                  \        4      ,
          WuP                  \        4      ,
          .4      8X  g   Q hVP                   V P"                  8X  g   Q hVP$                  P'                  VP(                  4      \+        ^ 4      8X  g   Q hVP,                  P'                  VP(                  4      \+        ^ 4      8X  g   Q hVP,                  P'                  VP.                  4      V) V P"                  ,          8X  g   Q hVP(                  P'                  VP(                  4      WPP"                  ,          8X  g   Q hVP$                  P1                  V 4      WpP"                  ,          8X  g   Q hVP3                  V 4      W`P"                  ,          8X  g   Q hVP4                  V 8X  g   Q hVP6                  V8X  g   Q hVP9                  4       R8X  g   Q h\        R4      w  rr\;        R4      w  r\        R4      w  rr#pp\        RW#WWV P"                  ,          WP<                  ,          VV^V P>                  ,          R7      pVP                  \        W.4      8X  g   Q hVP                  \        W.4      8X  g   Q hVP                  V	8X  g   Q hVP                  V8X  g   Q hVP                  V8X  g   Q hVP                  V8X  g   Q hVP                  \        WP                  \        4      ,
          WP                  \        4      ,
          .4      8X  g   Q hVP                   ^V P>                  ,          8X  g   Q hVP$                  P'                  VP(                  4      WP<                  ,          8X  g   Q hVP,                  P'                  VP(                  4      WP"                  ,          8X  g   Q hVP,                  P'                  VP.                  4      V) V P>                  ,          8X  g   Q hVP(                  P'                  VP(                  4      WP"                  ,          WPP>                  ,          ,           WP<                  ,          ,
          8X  g   Q hVP(                  P1                  V 4      WP>                  ,          W,          VP>                  ,          ,
          8X  g   Q hVP3                  V 4      WP>                  ,          8X  g   Q hR# )zq0:2_J, u0:2_Jr7   z(CylindricalJoint: J  parent: P  child: Cz
q0:2, u0:2l, mT)rotation_coordinaterotation_speedtranslation_speedrq   rr   rt   rw   rs   N) r1   r   r   rx   r4   r5   ry   r
   rW   r  translation_coordinater  r  rz   rY   rZ   rs   r+   rr   r{   r|   r   rq   r}   r   r   rt   rw   r~   r   r,   r   )r"   r#   r$   r'   q0_defq1_defu0_defu1_defCjrG   rH   rL   rM   r   r   r.   r/   s                    r0   test_cylindrical_jointr  K  s{   !JA!%34D%E"FF	#q	$B77c>>99>>88q==>>VV$4555599/0000!!V+++$$...&&&6)))77ff{{1~5vA7NOPPPP==ACC>>""1<<0F1I===??##ALL1VAY>>>??##BOO4!##EEE<<  .&33,>>>>>a FSSL000<<?fssl***1$$$!###::<EEEE#L1NBB6?DA+D1A!d	#q,.W&'##g+/AG
EB >>VRL111199x((((!!R'''$$..."""2%%%77fb771:orKKN/BCDDDD==AG###>>""1<<0AG;;;??##ALL1QW<<<??##BOO4!##EEE<<  	SS6CC</!cc'9: : :<<A"ss(RVacc\"9999<<?b33h&&&r2   c                     \        4       w  rr#\        R 4      w  rEp\        R4      w  rxp	\        RW#4      p
V
P                  R8X  g   Q hV
P                  V8X  g   Q hV
P
                  V8X  g   Q hV
P                  \        WEV.4      8X  g   Q hV
P                  \        WxV	.4      8X  g   Q hV
P                  V8X  g   Q hV
P                  \        WV.4      8X  g   Q hV
P                  V8X  g   Q hV
P                  \        W.4      8X  g   Q hV
P                  \        WtP                  \        4      ,
          WP                  \        4      ,
          WP                  \        4      ,
          .4      8X  g   Q hV
P                   V P"                  8X  g   Q hV
P$                  V P&                  V P(                  .8X  g   Q hV
P*                  P-                  VP.                  4      \1        ^ 4      8X  g   Q hV
P2                  P-                  VP.                  4      \1        ^ 4      8X  g   Q hWPP&                  ,          W`P(                  ,          ,           pV
P2                  P-                  V
P*                  4      V) 8X  g   Q hVP.                  P-                  VP.                  4      V8X  g   Q hV
P*                  P5                  V 4      WP&                  ,          WP(                  ,          ,           8X  g   Q hVP7                  V 4      WpP"                  ,          8X  g   Q hV
P8                  V 8X  g   Q hV
P:                  V8X  g   Q hV
P=                  4       R8X  g   Q h\        R4      w  rrpp\?        R4      w  pp\        R4      w  rr#pp\        RW#VW.VV.VV P"                  ,          VVP&                  ,          VVR7
      p
V
P                  \        WV.4      8X  g   Q hV
P                  \        VVV.4      8X  g   Q hV
P                  V8X  g   Q hV
P                  \        W.4      8X  g   Q hV
P                  V8X  g   Q hV
P                  \        VV.4      8X  g   Q hV
P                  \        W|P                  \        4      ,
          VVP                  \        4      ,
          VVP                  \        4      ,
          .4      8X  g   Q hV
P                   VP"                  8X  g   Q hV
P$                  VP&                  VP(                  .8X  g   Q hV
P*                  P-                  VP.                  4      VVP&                  ,          8X  g   Q hV
P2                  P-                  VP.                  4      VV P"                  ,          8X  g   Q hV
P2                  P-                  V
P*                  4      WP&                  ,          WP(                  ,          ,           8X  g   Q hVP.                  P-                  VP.                  4      VV P"                  ,          WP&                  ,          ,
          WP(                  ,          ,
          VVP&                  ,          ,
          8X  g   Q hVP.                  P5                  V 4      V) V P&                  ,          VV P(                  ,          ,
          VV,          VP"                  ,          ,           8X  g   Q hVP7                  V 4      WpP"                  ,          8X  g   Q hR# )	zq0:3_Jzu0:3_Jr7   z#PlanarJoint: J  parent: P  child: C
q0:3, u0:3r  T)r  planar_coordinatesplanar_speedsrq   rr   rt   rw   N) r1   r   r   rx   r4   r5   ry   r
   rW   r  r  r  r   rz   rY   rZ   rotation_axisr+   planar_vectorsr,   r   rr   r{   r|   r   rq   r   r   rt   rw   r~   r   )r"   r#   r$   r'   r  r  q2_defr  r  u2_defr  r_P_CrG   rH   rd   rL   rM   rf   r   r   r.   r/   s                         r0   test_planar_jointr&  |  su   !JA!+H5FF+H5FF	S!	B77c>>99>>88q==>>VVV$<====9978888!!V+++  FF+;$<<<<&&&vv&6777777ff{{1~5vA7N${{1~57 8 8 8 8qss"""acc
***>>""1<<0F1I===??##ALL1VAY>>>SSL6CC<'E??##BNN3v===<<  .%777>>a FSSL6CC<$????<<?fssl***1$$$!###::<@@@@+L9BBB6?DAq+D1A!d	S!B)+R"#acc'q133w'+d
DB >>VRRL111199B/0000!!R'''  FB8$4444&&&vr2h////77ffwwqz12
? 2771:o/ 0 0 0 0tvv%%% 0000>>""1<<0AG;;;??##ALL1QW<<<??##BNN3rCCx"ss(7JJJJ<<  	QSS28+b33h6QSS@A A A<<A2#)b133h"6!acc9I"IIII<<?fssl***r2   c                  (   \        R 4      w  rr#rE\        R4      w  rg\        4       w  rr\        RWWV.W4V.WhP                  ,          V) VP                  ,          VP                  VP
                  \        ^4      ,          ,           V	P                  V	P
                  \        ^4      ,          ,           R7      pVP                  P                  V4      VP                  VP
                  \        ^4      ,          ,           P                  4       8X  g   Q hVP                  P                  V	4      V	P                  V	P
                  \        ^4      ,          ,           P                  4       8X  g   Q hVP                  P                  VP                  4      \        ^,          8X  g   Q hVP                  V	4      P                  V ^ V^ V^ /4      \        ^4      8X  g   Q h\        \!        V 4      \        ^4      ) \#        V 4      ,          ^,          \#        V 4      ^,          .\        ^4      \#        V 4      ,          ^,          ^\!        V 4      ,          ^,          R,           \        ^4      ^\!        V 4      ,
          ,          ^,          .\#        V 4      ) ^,          \        ^4      ^\!        V 4      ,
          ,          ^,          \!        V 4      ^,          R,           ..4      p\%        VP                  V	4      V,
          4      \'        ^4      8X  g   Q hR# )r  zl1:3r7   )rq   rr   rt   rw   Ng      ?g      ?)r   r   r1   r   r   r,   r   r!  r   	normalizer   r   r   r   r   r
   r   r	   r   r   )rG   rH   rd   rL   rM   rf   l1l2r"   r#   r$   r'   r7   r   s                 r0   test_planar_joint_advancedr+    s9    ,L9BBBV_FB!JA!Cr8Rb!#cc!#acc	&'ccACC$q'M&9%&SS133a=%8		:A
 ??""1%!##d1g*=)H)H)JJJJ??""1%!##d1g*=)H)H)JJJJ??((-a777558b!RB23s1v===	R47(SW$q(#b'A+6	a3r7	Q	CGa% 7	aABK	 1	$	&
b'AtAw!c"g+.2CGaK%4GH	J KE AEE!Hu$%q111r2   c            	      ,   \        4       w  rr#\        R 4      w  rErgr\        RW#4      p
V
P                  R8X  g   Q hV
P                  V8X  g   Q hV
P
                  V8X  g   Q hV
P                  \        WEV.4      8X  g   Q hV
P                  \        WxV	.4      8X  g   Q hV
P                  \        WtP                  \        4      ,
          WP                  \        4      ,
          WP                  \        4      ,
          .4      8X  g   Q hV
P                  P                  VP                  4      \        ^ 4      8X  g   Q hV
P                   P                  VP                  4      \        ^ 4      8X  g   Q hV
P                   P                  V
P                  4      \        ^ 4      8X  g   Q hVP                  P                  VP                  4      \        ^ 4      8X  g   Q hVP                  P#                  V 4      \        ^ 4      8X  g   Q hV P%                  V4      V) \'        V4      ,          \'        V4      ,          V\)        V4      ,          ,
          VP*                  ,          V\)        V4      ,          \'        V4      ,          V\'        V4      ,          ,
          VP,                  ,          ,           V) \)        V4      ,          V	,
          VP.                  ,          ,           8X  g   Q hVP%                  V 4      V\'        V4      ,          \'        V4      ,          V\)        V4      ,          ,           VP*                  ,          V) \)        V4      ,          \'        V4      ,          V\'        V4      ,          ,           VP,                  ,          ,           V\)        V4      ,          V	,           VP.                  ,          ,           8X  g   Q hV
P0                  ! 4       R8X  g   Q hV
P2                  R8X  g   Q hV
P4                  ^{8X  g   Q hV
P6                  e   Q hR# )q0:3_S, u0:3_Sr   z&SphericalJoint: S  parent: P  child: CBODYN)r1   r   r   rx   r4   r5   ry   r
   rW   rz   rY   rZ   rr   r{   r|   r   rq   r   r   r   r	   r+   r,   r   r~   	_rot_type
_rot_order_amounts)r"   r#   r$   r'   rG   rH   rd   rL   rM   rf   r   s              r0   test_spherical_jointr2    s    !JA!+,<=BBBsA!A66S==88q==77a<<==FBB<000088vrrl++++66VR''!*_b771:orGGAJOPPPP==!!!,,/6!9<<<>>""1<<0F1I===>>""1==1VAY>>><<  .&);;;<<A&)+++<<?sSW}s2w6c"gELRL3r7"R#b'\1QSSP9 93R=2%<-- - - - <<?rCG|c"g5SWDKC#b'MCG#b3r7l2accO: :B<"$=,, , , , 99;BBBB;;&   <<3::r2   c            	         \        R 4      w  rp\        R ^4      w  r4p\        4       w  rgr\        RWWV.W4V.R7      p
V
P                  \	        WV.4      8X  g   Q hV
P
                  \	        W4V.4      8X  g   Q hV
P                  \	        . RO4      8X  g   Q hVP                  V4      V) \        V4      ,          \        V4      ,          V\        V4      ,          ,
          VP                  ,          V\        V4      ,          \        V4      ,          V\        V4      ,          ,
          VP                  ,          ,           V) \        V4      ,          V,
          VP                  ,          ,           8X  g   Q hR# )zq0:3r   )ry   rW   N)r   r   r   )r   r1   r   ry   r
   rW   rz   r   r   r	   r+   r,   r   )rG   rH   rd   rL   rM   rf   r"   r#   r$   r'   r   s              r0   /test_spherical_joint_speeds_as_derivative_termsr4    s,     'JBB*JBB!JA!sArrlBB<PA==FBB<000088vrrl++++66VI&&&&<<?sSW}s2w6c"gEL
SWs2wc"g-P5 5sSW}r!QSS8)) ) ) )r2   c                    a	a
aaaaaaaa \        R 4      w  rr#rE\        R4      w  oooooooo\        4       w  rgo
o	\        RS
S	SSS.SSS.4      pVP                  \	        SSS.4      8X  g   Q hVP
                  \	        SSS.4      8X  g   Q h\        4       w  rgo
o	\        RS
S	\	        SSS.4      \	        SSS.4      4      pVP                  \	        SSS.4      8X  g   Q hVP
                  \	        SSS.4      8X  g   Q h\        4       w  rgo
o	\        \        V	V
VVV3R l4       \        \        V	V
VVVVVVV3	R l4       \        \        V	V
VVVVVVV3	R l4       R# )r-  z
q0:4, u0:4r   c            	      L   < \        R SS \        SS.4      \        S.4      4      # r   r   r
   )r'   r$   rG   rH   rL   s   r0   r9   -test_spherical_joint_coords.<locals>.<lambda>	  s!    >#q!VRH-=vrd|Lr2   c                  T   <	 \        R SS \        SSSS.4      \        SSS.4      4      # r   r7  )	r'   r$   rG   rH   rd   re   rL   rM   rf   s	   r0   r9   r8    s/    ~Q62r2r*+VRRL-A Cr2   c                  T   <	 \        R SS \        SSS.4      \        SSSS.4      4      # r   r7  )	r'   r$   rG   rH   rd   rL   rM   rf   u4s	   r0   r9   r8    s/    ~Q62r2,'RR0@)A Cr2   N)r   r1   r   ry   r
   rW   r   rX   )q0sq1sq2su0su1su2sr"   r#   r   r'   r$   rG   rH   rd   re   rL   rM   rf   r;  s            @@@@@@@@@@r0   test_spherical_joint_coordsrB    sY   #12B#C Cc%3L%A"BBBB!JA!QsAq2r2,R=A==FBB<000088vr2rl++++!JA!QsAq&"b""6r2rl+	-A==FBB<000088vr2rl++++!JA!Q
:LLN : C C D
: C C Dr2   c                    aaaaaa \        R 4      w  oooooo\        \        S4      ) \        S4      ) \        S4      ,          \        S4      \        S4      ,          .\        S4      ) \        S4      ,          \        S4      \        S4      ,          \        S4      ,          \        S4      \        S4      ,          ,
          \        S4      ) \        S4      ,          \        S4      ,          \        S4      \        S4      ,          ,
          .\        S4      \        S4      ,          \        S4      ) \        S4      ,          \        S4      \        S4      ,          \        S4      ,          ,
          \        S4      ) \        S4      ,          \        S4      \        S4      ,          \        S4      ,          ,           ..4      p \        S) \        S4      ,          S,
          .S) \        S4      ,          \        S4      ,          S\        S4      ,          ,           .S\        S4      ,          \        S4      ,          S\        S4      ,          ,           ..4      p\        \	        ^4      ) S\        S\
        ^,          ,           4      ,          \        S4      ,          S\        S\
        ^,          ,           4      ,          ,           ,          .S) \        S4      ,          S,
          .S) \        S4      ,          S,
          ..4      p\        R4      w  r4rVrx\        RWVSSS.SSS.VP                  VP                  ,           VP                  ) VP                  ,           WxR^{R7      p	V	P                  P                  4       R8X  g   Q hV	P                  ^{8X  g   Q h\        VP                  V4      V ,
          4      \!        ^4      8X  g   Q h\        VP#                  V4      P%                  V4      V,
          4      \!        ^^4      8X  g   Q h\        VP&                  P)                  V4      P%                  V4      4      V8X  g   Q h\        R4      w  r4rVrx\        RWVSSS.SSS.VP                  VP                  ,           VP                  ) VP                  ,           WxRSSS3^{R7      p	VVVVVV3R lp
V	P                  P                  4       R8X  g   Q hV	P                  ^{8X  g   Q h\        VP                  V4      V
! V 4      ,
          4      \!        ^4      8X  g   Q h\        VP#                  V4      P%                  V4      V
! V4      ,
          4      \!        ^^4      8X  g   Q h\        VP&                  P)                  V4      P%                  V4      4      V
! V4      8X  g   Q h\        R4      w  r4rVrx\        RWVSSS.SSS.VP                  VP                  ,           VP                  ) VP                  ,           WxRR	R7      p	V	P                  P                  4       R8X  g   Q hV	P                  R	8X  g   Q h\        VP                  V4      \        \        S4      ) \        S4      ,          \        S4      \        S4      ,          \        S4      ,          \        S4      \        S4      ,          ,
          \        S4      \        S4      ,          \        S4      ,          \        S4      \        S4      ,          ,           .\        S4      ) \        S4      ) \        S4      ,          \        S4      ) \        S4      ,          .\        S4      \        S4      ,          \        S4      ) \        S4      ,          \        S4      \        S4      ,          \        S4      ,          ,
          \        S4      \        S4      ,          \        S4      \        S4      ,          \        S4      ,          ,
          ..4      ,
          4      \!        ^4      8X  g   Q h\        VP#                  V4      P%                  V4      \        S\        S4      ,          S,
          .S\        S4      ,          \        S4      ,          S\        S4      ,          ,
          .S\        S4      ,          \        S4      ,          S\        S4      ,          ,           ..4      ,
          4      \!        ^^4      8X  g   Q h\        VP&                  P)                  V4      P%                  V4      4      \        \	        ^4      ) S\        S\
        ^,          ,           4      ,          \        S4      ,          S\        S\
        ^,          ,           4      ,          ,           ,          .S\        S4      ,          S,
          .S\        S4      ,          S,
          ..4      8X  g   Q hR
# )r  Tr   bodyry   rW   rq   rr   rt   rw   rot_type	rot_orderr.  	ry   rW   rq   rr   rt   rw   rF  amountsrG  c                 >   < V P                  SSSSSSSSSSSS/4      # r   r   exprrG   rH   rd   rL   rM   rf   s   &r0   r9   2test_spherical_joint_orient_body.<locals>.<lambda>0  *    	RRRRRR8!:r2   BodYyxzN)r   r
   r	   r   r   r   r1   r   r+   r,   r   r/  upperr0  r   r   r   r   r   r|   r   r   N_w_AN_v_Cor"   r#   r$   r'   r.   r/   r   switch_orderrG   rH   rd   rL   rM   rf   s              @@@@@@r0    test_spherical_joint_orient_bodyrW    sp   +L9BBB
b'CG8c"g%s2wR'89
b'CG	SWs2w.R83r7SW;LL
b'CG	c"g	%B#b'(9	9	;	R3r7	SWHs2w.R3r71BSW1LL
b'CG	c"gB/#b'9	9	;	< =E bS3r7]R'(S3r7]SW,rCG|;<#b'\CG+b3r7l:;= >E 
q'R#b26k**SW4rCR!V<L7LL	MN
s2w	sSW}r124 5F ,D1A!sAr2rlBB<$%CC!##IACC4!##:)- &#	7A ;;&(((<<3AEE!Hu$%q111ALLO--a0589U1a[HHHALL$$Q'11!45???+D1A!sAr2rlBB<$%CC!##IACC4!##:)- &R	MA: :L;;&(((<<3AEE!H|E223uQx???ALLO--a0<3FF !&q!- - -ALL$$Q'11!45f9MMMM+D1A!sAr2rlBB<$%CC!##IACC4!##:)- &%	9A ;;&(((<<5   AEE!Hv
b'CG	SWs2w.R83r7SW;LL	R3r7	SW	$s2wR'8	8	:
b'CG8c"g%Bx#b''9:	R3r7	SWHs2w.R3r71BSW1LL	R3r7	SWs2w.R8	8	:	';  < < =
 AFaI I I ALLO--a06	c"g	b3r7lSW4rCG|CD	c"gB	"s2w,	./;1 42 2 36;AqkB B B ALL$$Q'11!45
q'R#b26k**SW4rCR!V<L7LL	MN	c"g	b3r7lR/0A2 :3 3 3 3r2   c                    aaaaaa \        R 4      w  oooooo\        \        S4      ) \        S4      ,          \        S4      \        S4      ,          \        S4      ,          ,
          \        S4      \        S4      ,          \        S4      ,          \        S4      \        S4      ,          ,
          \        S4      \        S4      ,          .\        S4      ) \        S4      ,          \        S4      \        S4      ,          \        S4      ,          ,           \        S4      ) \        S4      ,          \        S4      ,          \        S4      \        S4      ,          ,
          \        S4      ) \        S4      ,          .\        S4      \        S4      ,          \        S4      ) \        S4      ,          \        S4      ..4      p \        S\        S4      ,          S\        S4      ,          \        S4      ,          ,
          .S\        S4      ,          S\        S4      ,          \        S4      ,          ,           .SS\        S4      ,          ,
          ..4      p\        SS\        S4      ,          ,
          .SS\        S4      ,          ,
          .\	        ^4      S) \        S\
        ^,          ,           4      ,          S\        S\
        ^,          ,           4      ,          \        S4      ,          ,           ,          ..4      p\        R4      w  r4rVrx\        RWVSSS.SSS.VP                  VP                  ,           VP                  ) VP                  ,           WxR^{R7      p	V	P                  P                  4       R8X  g   Q hV	P                  ^{8X  g   Q h\        VP                  V4      V ,
          4      \!        ^4      8X  g   Q h\        VP#                  V4      P%                  V4      4      V8X  g   Q h\        VP&                  P)                  V4      P%                  V4      4      V8X  g   Q hVVVVVV3R lp
\        R4      w  r4rVrx\        RWVSSS.SSS.VP                  VP                  ,           VP                  ) VP                  ,           WxRSSS3^{R7      p	V	P                  P                  4       R8X  g   Q hV	P                  ^{8X  g   Q h\        VP                  V4      V
! V 4      ,
          4      \!        ^4      8X  g   Q h\        VP#                  V4      P%                  V4      4      V
! V4      8X  g   Q h\        VP&                  P)                  V4      P%                  V4      4      V
! V4      8X  g   Q h\        R4      w  r4rVrx\        RWVSSS.SSS.VP                  VP                  ,           VP                  ) VP                  ,           WxRR	R7      p	V	P                  P                  4       R8X  g   Q hV	P                  R	8X  g   Q h\        VP                  V4      \        \        S4      ) \        S4      ,          \        S4      ) \        S4      ,          \        S4      \        S4      ,          \        S4      ,          ,           \        S4      \        S4      ,          \        S4      ,          \        S4      \        S4      ,          ,           .\        S4      ) \        S4      ) \        S4      ,          \        S4      ) \        S4      ,          .\        S4      \        S4      ,          \        S4      ) \        S4      ,          \        S4      \        S4      ,          \        S4      ,          ,
          \        S4      ) \        S4      ,          \        S4      ,          \        S4      \        S4      ,          ,           ..4      ,
          4      '       g   Q h\        VP#                  V4      P%                  V4      \        S) S\        S4      ,          ,           .S) \        S4      ,          S\        S4      ,          \        S4      ,          ,           .S\        S4      ,          S\        S4      ,          \        S4      ,          ,           ..4      ,
          4      \!        ^^4      8X  g   Q h\        VP&                  P)                  V4      P%                  V4      \        S\        S4      ,          S\        S4      ,          \        S4      ,          ,           .S\        S4      ,          S\        S4      ,          \        S4      ,          ,           .SS\        S4      ,          ,           S\        S4      ,          ,
          S\        S4      ,          \        S4      ,          ,
          ..4      ,
          4      \!        ^^4      8X  g   Q hR
# )r  Tr   spacerE  SPACEc                 >   < V P                  SSSSSSSSSSSS/4      # r   rK  rL  s   &r0   r9   3test_spherical_joint_orient_space.<locals>.<lambda>h  rO  r2   rH  SPaCezxyN)r   r
   r	   r   r   r   r1   r   r+   r   r,   r/  rR  r0  r   r   r   r   r   r|   r   rS  s              @@@@@@r0   !test_spherical_joint_orient_spacer_  N  su   +L9BBB
b'CG	c"gB/#b'9	9	R3r7	SW	$s2wR'8	8#b'CG:K	M
b'CG	c"gB/#b'9	9
b'CG	c"g	%B#b'(9	9CG8c"g;M	O	R3r7	SWHs2w.B8: ;E 	c"gSWs2w.	./	c"gSWs2w.	./"rCG|2C1DF GE 	b3r7l	b2B</0	aRC#b26k**R#b26k2B-BSW-LL	MNP QF ,D1A!sAr2rlBB<$%CC!##IACC4!##:)- '3	8A ;;')))<<3AEE!Hu$%q111ALLO--a01U:::ALL$$Q'11!45???: :L+D1A!sAr2rlBB<$%CC!##IACC4!##:)- '"b"	NA ;;')))<<3AEE!H|E223uQx???ALLO--a01\%5HHHHALL$$Q'11!45f9MMMM+D1A!sAr2rlBB<$%CC!##IACC4!##:)- '5	:A ;;')))<<5   AEE!Hv
b'CG	c"gXB/#b'CG2Cc"g2MM	R3r7	SW	$s2wR'8	8	:
b'CG8c"g%Bx#b''9:	R3r7	SWHs2w.R3r71BSW1LL
b'CG	c"g	%B#b'(9	9	;	'<  = = > > > > ALLO--a06
rCG|	sSW}rCG|c"g/EEF	c"gSWs2w.	./;1 42 2 36;AqkB B B ALL$$Q'11!4v	c"gSWs2w.	./	c"gSWs2w.	./	b3r7l	R#b'\	)	c"gB	
  	!?" 8# # $ (-Q{	3 3 3r2   c            	      B
   \        4       w   rp\        R W4      pVP                  R 8X  g   Q hVP                  V8X  g   Q hVP                  V8X  g   Q hVP
                  \        4       8X  g   Q hVP                  \        4       8X  g   Q hVP                  \        ^^ . 4      P                  8X  g   Q hVP                  P                  VP                  4      \        ^4      8X  g   Q hVP                  P                  VP                  4      \!        ^ 4      8X  g   Q hVP"                  P                  VP                  4      \!        ^ 4      8X  g   Q hVP"                  P                  VP                  4      \!        ^ 4      8X  g   Q hVP                  P                  VP                  4      \!        ^ 4      8X  g   Q hVP                  P%                  VP                  4      \!        ^ 4      8X  g   Q hVP                  P'                  VP                  4      ^ 8X  g   Q hVP                  P'                  VP                  4      ^ 8X  g   Q hVP)                  4       R8X  g   Q h\        4       w  rEr\+        R4      w  rg\-        R4      pVP/                  VP                  VP0                  \2        ^,          4       \        R WWaP                  P4                  ,          WrP                  P0                  ,          VR7      pVP                  P                  VP                  4      WrP                  P0                  ,          8X  g   Q hVP"                  P                  VP                  4      WaP                  P4                  ,          8X  g   Q hVP"                  P                  VP                  4      \!        ^ 4      8X  g   Q hVP                  P                  VP                  4      V) VP4                  ,          WuP0                  ,          ,           8X  g   Q hVP                  P%                  VP                  4      \!        ^ 4      8X  g   Q hVP                  P%                  V4      \!        ^ 4      8X  g   Q hVP                  P'                  VP                  4      ^ 8X  g   Q hVP                  P'                  VP                  4      ^ 8X  g   Q hVP4                  VP6                  8X  g   Q h\9        4       ;_uu_ 4        \;        W4       RRR4       R#   + '       g   i     R# ; i)Wz!WeldJoint: W  parent: P  child: Crk   r(   )rq   rr   rt   N)r1   r   rx   r4   r5   ry   r
   rW   rz   Tr&   r   r   rr   r{   r|   r   rq   r   r   r~   r   r   r*   r,   r   r+   r   r    r   )	rh   r$   r'   ra  r"   r#   r   r   r.   s	            r0   test_weld_jointrc    s   !JAqQ#qA66S==88q==77a<<==FH$$$88vx66VAq"%'''''77;;qww3q6)))==!!!,,/6!9<<<>>""1<<0F1I===>>""1==1VAY>>><<  .&);;;<<AGG$q	11177agg&!+++77agg&!+++99;====!JA!5>DA'"DQWWacc26*#q!ggii-''))mt	EA ==!!!,,/1wwyy=@@@>>""1<<0A		MAAA>>""1==1VAY>>><<  .A#)a##g2EEEE<<AGG$q	111<<D!VAY...77agg&!+++77agg&!+++33!##::		!	!Q 
"	!	!	!s   7TT	c            
         \        R 4      w  r\        4       w  r#rE\        4       ;_uu_ 4        \        RWEVP                  ) R7       RRR4       VP                  ) P                  VP                  4      ^ 8X  g   Q hVP                  P                  V4      ) VP                  8X  g   Q hVP                  V4      \        . RO^ \        V 4      ) \        V 4      ) .^ \        V 4      ) \        V 4      ..4      8X  g   Q hVP                  V4      WP                  ,          8X  g   Q hVP                  V4      P                  4       \        V^,          4      8X  g   Q h\        4       w  r#rE\        4       ;_uu_ 4        \        RWEVP                  VP                  ,           R7       RRR4       VP                  P                  VP                  VP                  ,           4      ^ 8X  g   Q hVP                  P                  V4      VP                  VP                  ,           \        ^4      ,          8X  g   Q hVP                  V4      \        \        ^4      ^,          \        ^4      ^,          ^ .\        ^4      ) ^,          \        ^4      ^,          ^ .. RO.4      8X  g   Q hVP                  V4      \!        ^ 4      8X  g   Q hR#   + '       g   i     EL; i  + '       g   i     EL?; i)rl   r7   )
child_axisN)parent_axisr   r   )r   r1   r    r   r+   r   r   r   r
   r   r	   r   r   r   r   r,   r   )rA   r?   r"   r#   r$   r'   s         r0   !test_deprecated_parent_child_axisrg    s2   *%DA!JA!		!	!at, 
"SSD$)))CCKKN?acc!!!558vz !CF7SVG4 !CF7CF35 6 6 6 6 <<?a##g%%%<<?$$&$qAv,666!JA!		!	!sAaccACCi8 
"CCqssQSSy)Q...33;;q>accACCi472222558vQ!T!Wq[!<!%a1d1gk1=yJ K K K K<<?fQi'''# 
"	!	! 
"	!	!s   K9*KK	K*	c            
      @   \        4       w  rr#\        4       ;_uu_ 4        \        R W#V P                  V P                  ,           VP                  VP
                  ,
          R7      pRRR4       XP                  P                  VP                  4      V P                  V P                  ,           8X  g   Q hVP                  P                  VP                  4      VP                  VP
                  ,
          8X  g   Q h\        4       w  rr#\        4       ;_uu_ 4        \        R W#V P
                  V P                  ,           VP                  VP                  ,
          R7      pRRR4       XP                  P                  VP                  4      V P
                  V P                  ,           8X  g   Q hVP                  P                  VP                  4      VP                  VP                  ,
          8X  g   Q hR#   + '       g   i     EL; i  + '       g   i     L; i)r7   )parent_joint_poschild_joint_posN)r1   r    r   r+   r,   r   rq   r{   r|   rr   r   )r"   r#   r$   r'   r   sliders         r0   test_deprecated_joint_posrl    s_   !JA!		!	!sA1339'(ssQSSy2 
" $$Q\\2accACCi?????##ALL1QSS133Y>>>!JA!		!	!QACC!##I01acc	; 
" ''5qssBBB&&q||4acc	AAA 
"	!	! 
"	!s   AG9AH9H
	H	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    _trZ   r1   r<   ri   r   r   r   r   r   r   r   r   r   r   r  r  r  r  r  r&  r+  r2  r4  rB  rW  r_  rc  rg  rl   r2   r0   <module>rz     s    * ! " 9 ? . . %D D D D 0 > > ? 
9,O^6&rF6R5"%AP,?^I/X5"pLD^"$H.M<K03,la H.'b4+n266)D2:3z;3|%P(.Br2   