
    \hB                        S SK Jr  S SKJr  S SKJr  S SKJr  S SKJ	r	J
r
  S SKJrJrJrJr  S SKJr  S SKJrJrJrJrJrJrJrJrJrJrJr  S S	KJr  S S
K J!r!J"r"J#r#  S SK$J%r%J&r&  \RN                  r(S)S jr)S r*S r+S r,S r-S r.S r/S r0S r1S r2S r3S r4S r5S r6S r7S r8S r9S r:S r;S r<S  r=S! r>S" r?S# r@S$ rAS% rBS& rCS' rDg()*    )
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                 (   [        S5      n[        S5      n[        SUS9n[        SUS9nU (       a^  [        S5      [        S5      peUR                  XR                  [        5        UR                  X"R
                  [        * S-  5        XX4XV4$ XX44$ )	NNAPframeCP_intC_int   )r   r   orient_axisxr   y)
interframer!   r"   r#   r&   PintCints          `/var/www/auris/envauris/lib/python3.13/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                  ^   ^ ^ [        S5      m[        S5      m [        [        U U4S j5        g )Nparentchildc                     > [        STT 5      $ NJr   r5   r4   s   r0   <lambda>test_Joint.<locals>.<lambda>#   s    eC7r2   )r   r   	TypeErrorr9   s   @@r0   
test_Jointr=       s"    x FgE
978r2   c            	        ^^^^^^^^ [        S5      u  pp#[        S5      u  nmpVpxp[        S5      u  mmpmmp[        5       u    nmm[        STT5      mTR                  U S5      [	        U /5      :X  d   eTR                  U/S5      [	        U/5      :X  d   eTR                  U/SSS9[	        U/5      :X  d   eTR                  S S5      [	        U/5      :X  d   eTR                  S /S5      [	        U/5      :X  d   eTR                  TS S /S5      [	        TTU/5      :X  d   eTR                  S S5      [	        UTU/5      :X  d   eTR                  / S5      [	        UTU/5      :X  d   eTR                  / SSS9[	        TXV/5      :X  d   eTR                  TS U/SSS9[	        TX\/5      :X  d   eTR                  S SSS9[	        XV/5      :X  d   eTR                  S SS	5      [	        U/5      :X  d   eTR                  / SS	5      [	        XxU	/5      :X  d   eTR                  S SS
S9[	        U/5      :X  d   eTR                  / SS	SS
5      [	        U	/5      :X  d   eTR                  / SS5      [	        UTU/5      :X  d   e[        [        UUU4S j5        [        [        UUU4S j5        [        [        UUU4S j5        [        [        UU4S j5        [        [        UUU4S j5        [        5       u    nmm[        STTTTR                  [        5      5        [        5       u    nmm[        [        UUU4S j5        [        [        UUUU4S j5        g )Nzq u q_J u_Jzq0:4_J u0:4_Jz	q0:4 u0:4r8      r)   )offset   uT)number_singleqc                  ,   > T R                  TT/S5      $ )Nr?   _fill_coordinate_listr8   q0q1s   r0   r:   ,test_coordinate_generation.<locals>.<lambda>F       q66BxCr2   c                  0   > T R                  TTS /SS5      $ )Nr)   rB   rF   )r8   u0u1s   r0   r:   rK   G   s    q66B~q#Nr2   c                  ,   > T R                  TT/S5      $ )NrA   rF   rH   s   r0   r:   rK   H   rL   r2   c                  >   > T R                  T[        S5      /S5      $ )NrJ   r)   )rG   r   )r8   rI   s   r0   r:   rK   J   s    a55r74=6I1Mr2   c                  2   > T R                  TT-   T/S5      $ )Nr)   rF   rH   s   r0   r:   rK   K   s    a55rBwmQGr2   c                  &   > [        STT TS S /5      $ r7   r   )r&   r#   q1js   r0   r:   rK   Q   s    ~c1a#tT9JKr2   c                  "   > [        STT TTT/S9$ )Nr8   )speedsrT   )r&   r#   rN   rO   s   r0   r:   rK   R   s    ~c1aRMr2   )
r   r1   r   rG   r	   r   
ValueErrorr<   difft)rD   rB   qjujq0jq2jq3ju0ju1ju2ju3jq2q3u2u3_r&   r8   r#   rI   rJ   rU   rN   rO   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                  N   [        S5      n [        S5      n[        S5      u  p#[        S5      u  pE[        SX5      nUR                  S:X  d   eUR
                  U :X  d   eUR                  U:X  d   eUR                  [        U/5      :X  d   eUR                  [        U/5      :X  d   eUR                  [        XTR                  [        5      -
  /5      :X  d   eUR                  U R                  R                  :X  d   eUR                   R#                  UR$                  5      ['        S5      :X  d   eUR(                  R#                  U R$                  5      ['        S5      :X  d   eUR(                  R#                  UR*                  5      ['        S5      :X  d   eUR$                  R#                  U R$                  5      ['        S5      :X  d   eUR,                  U R                  :X  d   eUR.                  UR                  :X  d   eUR1                  5       S:X  d   e[        S5      n[        S	5      n[3        S
5      n	U	R5                  UR                  UR6                  [8        S-  5        [        SXxX'R                  R                  -  X8R                  R6                  -  UR                  R:                  U	S9n
U
R<                  UR                  R:                  :X  d   eU
R*                  R#                  UR$                  5      X8R                  R6                  -  :X  d   eU
R>                  R#                  UR$                  5      X'R                  R                  -  :X  d   eU
R>                  R#                  U
R*                  5      ['        S5      :X  d   eUR$                  R#                  UR$                  5      U* UR                  R                  -  X8R                  R6                  -  -   :X  d   eU
R,                  U	:X  d   eU
R.                  UR                  :X  d   e[        S5      u  pE[A        S5      u  pppU R$                  RC                  SUR                  UR6                  -   5      nUR$                  RC                  SUR6                  UR:                  -   5      n[        SXXEUXXR:                  S9
nUR                  UR:                  :X  d   eUR(                  RE                  U5      S:X  d   eUR(                  U:X  d   eUR                   U:X  d   eUR                   R#                  U R$                  5      UR                  UR6                  -   :X  d   eUR(                  R#                  UR$                  5      UR6                  UR:                  -   :X  d   eUR$                  R#                  U R$                  5      UR                  UR6                  -   UR6                  -
  UR:                  -
  :X  d   eUR$                  RE                  U5      RG                  U5      U[I        U5      -  U[K        U5      -  -
  UR                  -  U* [I        U5      -  U[K        U5      -  -
  UR6                  -  -   :X  d   eUR,                  U	:X  d   eUR.                  U:X  d   eg )Nr#   r&   l mq_J, u_Jr8   r   z PinJoint: J  parent: P  child: CP1C1r'   r)   J1parent_pointchild_point
joint_axisparent_interframeq, uTrq   rr   rq   rr   rt   child_interframers   )&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mrD   rB   Pjrm   rn   r.   ro   r!   r"   r/   rq   rr   r8   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                  6   [        S5      u  p[        S5      n[        S5      n[        SU S9n[        S5      u  pV[	        SX4XVUXR
                  -  S9nUR                  U:X  d   eUR                  R                  S:X  d   eUR                  R                  UR                  5      U* UR
                  -  :X  d   eUR                  UR                  5      [        / S	QS
[        U5      [        U5      /S
[        U5      * [        U5      //5      :X  d   eUR                  R                  UR                  5      U* U-  UR                   -  :X  d   e[        S5      n[        S5      n[        S5      n[        SU S9n[        S5      u  pV[	        SX4XVUX!UR
                  -  UR                   S9	nUR"                  UR                  R                   :X  d   eUR                  UR                  5      [        [        U5      [        U5      S
/[        U5      * [        U5      S
// SQ/5      :X  d   eUR                  R                  UR                  5      S
:X  d   eUR                  R                  UR                  5      X-  UR$                  -  :X  d   e[        S5      u  pppX4X4X44 VVs0 s H  u  nnUR'                  [(        5      U_M     nnn[        S5      [        S5      pC[+        SX4XV5      nUR                  R                  UR                  5      [-        S5      :X  d   eUR                  R                  UR                  5      XWR                  R$                  -  :X  d   eUR                  R                  UR                  5      S
:X  d   eUR                  R                  UR                  5      XgR                  R$                  -  :X  d   e[        S5      [        S5      pC[        S5      n[/        SX4XXUXR$                  -  UR
                  S9
nUR                  R                  UR                  5      [        [        U	5      S
[        U	5      // SQ[        U	5      * S
[        U	5      //5      :X  d   eUR                  R                  UR                  5      XR$                  -  XR
                  -  -   :X  d   eUR                  R                  UR                  5      XR
                  -  :X  d   eUR                  R                  UR                  5      R1                  U5      U* UR
                  -  X-  UR                   -  -
  :X  d   e[        S5      [        S5      pC[        S5      n[3        SX4XU/XU/X!UR                   -  S9	nUR                  nUR                  R                  UR                  5      [        / S	QS
[        U	5      [        U	5      * /S
[        U	5      [        U	5      //5      :X  d   eUR                  R                  UR                  5      U* UR                   -  XR
                  -  -   XR                   -  -   :X  d   eUR                  R                  UR                  5      X-  UR
                  -  XR
                  -  -   XR                   -  -   :X  d   e[        S5      [        S5      pC[        S5      [        S5      p[5        SX4UX!UR$                  -  XR
                  -  S9nUR                  U5      [-        S5      :X  d   eUR                  R                  UR                  5      XR$                  -  XR
                  -  -
  :X  d   eUR                  R                  UR                  5      S
:X  d   eg s  snnf )Nm lr&   r#   )massru   r8   rw   rr   	J_P_framer?   r   r   r   )rt   rw   rr   rs   r   r   r?   z	q1:4 u1:4rA   P_frame)rt   rq   rs   r   r?   r   C_frame)rt   rw   rq   rr   )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&   rD   rB   r8   r   rJ   rd   re   rO   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   -#^c            
         [        S5      u  p[        S5      n[        5          [        S5      n[        SXS9nS S S 5        [	        S5      u  pV[        SWWXVXR                  -  S9  UR                  U:X  d   eUR                  R                  S:X  d   eUR                  R                  UR                  5      U* UR                  -  :X  d   eUR                  R                  UR                  5      [        / S	QS
[        U5      [        U5      /S
[        U5      * [        U5      //5      :X  d   eUR                  R                  UR                  5      U* U-  UR                   -  :X  d   eg ! , (       d  f       GN== f)Nr   r&   r#   r   r%   ru   r8   rr   r   r   r   )r   r   r   r   r   r   r,   r%   rx   r|   r{   r   r	   r   r   r   r   )r   r   r   r#   r&   rD   rB   s          r0   test_body_compatibilityr      sD   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   E//
E>c            
         [        S5      u  p[        S5      u  p#[        S5      u  pE[        S5      n[        S5      n[        S5      n[        SUS9n	[        S	XtS
9n
[        SXS
9n[	        SXX U* UR
                  -  U	R                  R                  S9n[	        SXX1U* UR
                  -  U
R                  R                  S9nUR                  U5      [        [        U 5      [        U 5      * S/[        U 5      [        U 5      S// SQ/5      :X  d   eUR                  U5      [        [        U5      [        U5      * S/[        U5      [        U5      S// SQ/5      :X  d   e[        UR                  U5      5      [        [        X-   5      [        X-   5      * S/[        X-   5      [        X-   5      S// SQ/5      :X  d   eUR                  U5      X&R                  -  :X  d   eUR                  U5      X7R                  -  :X  d   eUR                  U5      X&R                  -  X7R                  -  -   :X  d   eUR                  [        X R                  [         5      -
  /5      :X  d   eUR                  [        X1R                  [         5      -
  /5      :X  d   eU
R"                  R%                  U5      XR-  UR&                  -  :X  d   eUR"                  R%                  U5      XS-  UR&                  -  :X  d   eUR"                  R%                  U5      XR-  UR&                  -  XRU-   -  UR&                  -  -   :X  d   eg )Nzq1 q2zu1 u2r   r!   r"   Br&   r$   r#   r%   r   Rro   )rW   ry   rr   rs   J2r   r   )r   r   r   r   r   r+   r%   r   r   r	   r   r   r
   
ang_vel_inrz   rY   rZ   r|   r   r,   )rJ   rd   rO   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            
         [        S5      u  pp#n[        S5      u  pVpx[        S5      n	[        S5      n
[        S5      nX4S-  -
  S-  nUS-  US-  -   n[        SXS	9n[        S
XS9n[        SU	S9n[	        SXXWX*R
                  -  U	R                  S9n[	        SXXhXR
                  -  U
R
                  S9nU
R                  U	5      [        [        U5      S[        U5      * // SQ[        U5      S[        U5      //5      :X  d   eU
R                  U5      [        [        U5      [        U5      * S/[        U5      [        U5      S// SQ/5      :X  d   eUR                  U	5      [        [        U5      [        U5      -  [        U5      [        U5      * [        U5      -  /[        U5      * [        U5      -  [        U5      [        U5      [        U5      -  /[        U5      S[        U5      //5      :X  d   eU
R                  U	5      XyR                  -  :X  d   eU
R                  U5      U* U
R
                  -  :X  d   eU	R                  U5      U* U	R                  -  XR
                  -  -
  :X  d   eUR                  [        XuR                  [        5      -
  /5      :X  d   eUR                  [        XR                  [        5      -
  /5      :X  d   eUR                  R!                  UR                  5      X*R
                  -  :X  d   eUR                  R!                  UR                  5      U* U
R
                  -  :X  d   eUR                  R#                  U	5      US-  US-  -
  U-  U
R$                  -  :X  d   eUR                  R#                  U	5      US-  US-  -
  U-  US-  US-  -   U-  -   U
R$                  -  :X  d   eg )NzmA, mB, lA, lB, hztheta phi omega alphar!   r"   r   r)      rodr   plater   r&   r$   ro   )ry   rW   rr   rs   r   )ry   rW   rq   rs   r   r   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                    ^^^^ [        S5      u  p[        5       u  p#mm[        S5      [        S5      smm[        [        UUU4S j5        [        [        UUU4S j5        TR                  X"R                  U 5        TR                  X3R                  U 5        [        [        UUU4S j5        [        [        UUU4S j5        [        5       u  p#mm[        S5      mTR                  U[        S-  [        [        S	-  4S
5        [        STTXUR                  TR                  * TTR                  S9	  [        UR                  U5      5      [        S[        S	5      [!        U 5      -  S-  [        S	5      * [#        U 5      -  S-  /[        S5      S-  [        S5      S[#        U 5      -  [!        U 5      -   -  S-  [        S5      [#        U 5      * S[!        U 5      -  -   -  S-  /[        S5      S-  [        S5      S[#        U 5      -  [!        U 5      -   -  S-  [        S5      * [#        U 5      S[!        U 5      -  -   -  S-  //5      -
  [%        S	5      :X  d   eUR'                  U5      UTR                  -  :X  d   eTR(                  R+                  TR(                  5      UR                  UR                  -   :X  d   eTR(                  R-                  U5      XR                  -  :X  d   eTR(                  R-                  T5      [/        S5      :X  d   eTR(                  R-                  T5      XR                  -  :X  d   e[        5       u  p#mm[        S5      mTR                  US[        -  S	-  [        * [        S-  4S
5        [        STTXUR                  * TR                  TTR                  TR                  -   S9	  [        UR                  U5      5      [        [        S5      * [#        U 5      -  S-  [        S	5      * [!        U 5      S-
  -  S-  [!        U 5      S-  -
  [1        S5      S-  -
  [        S	5      [!        U 5      S-   -  S-  [!        U 5      S-  -
  [1        S5      S-  -   /[!        U 5      [        S5      [        S5      -   [#        U 5      * -  S-  [        S5      * [        S5      -   [#        U 5      -  S-  /[        S5      [#        U 5      -  S-  [        S	5      [!        U 5      S-   -  S-  [!        U 5      S-  -   [1        S5      S-  -
  [        S	5      S[!        U 5      -
  -  S-  [!        U 5      S-  -   [1        S5      S-  -   //5      :X  d   eUR'                  U5      [        S5      U-  S-  UR                  -  [        S5      U-  S-  UR                  -  -   :X  d   eTR(                  R+                  TR(                  5      UR                  * UR                  -
  :X  d   eTR(                  R-                  U5      R                  5       [        S5      * [        S5      -
  U-  S-  UR                  -  [        S5      * [        S5      -   U-  S-  UR                  -  -   :X  d   eTR(                  R-                  T5      [/        S5      :X  d   e[        5       u  p#mm[        S5      [        S5      smmTR                  U[        * S-  [        [        S-  4S
5        TR                  US[        -  S	-  [        * [        S-  4S
5        [        STTXUR                  UR                  -
  TR                  * TTTR                  TR                  -   S9
  [        UR                  U5      5      [        [!        U 5      [        S5      [        S5      -   [#        U 5      * -  S-  [        S5      * [        S5      -   [#        U 5      -  S-  /[        S5      * [#        U 5      -  S-  [        S	5      * [!        U 5      S-   -  S-  [!        U 5      S-  -
  [1        S5      S-  -   [        S	5      [!        U 5      S-
  -  S-  [!        U 5      S-  -
  [1        S5      S-  -
  /[        S5      [#        U 5      -  S-  [        S	5      [!        U 5      S-
  -  S-  [!        U 5      S-  -   [1        S5      S-  -   [        S	5      * [!        U 5      S-   -  S-  [!        U 5      S-  -   [1        S5      S-  -
  //5      :X  d   eUR'                  U5      [        S5      U-  S-  TR                  -  [        S5      U-  S-  TR                  -  -   :X  d   eTR(                  R+                  TR(                  5      UR                  UR                  -
  UR                  -   :X  d   e[        S5      * [        S5      -   U-  S-  UR                  -  nTR(                  R-                  U5      R                  5       U:X  d   eTR(                  R-                  T5      R                  5       U:X  d   eTR(                  R-                  T5      [/        S5      :X  d   eg )Nru   r.   r/   c                     > [        STT TS9$ Nr8   rt   r   r&   r#   r.   s   r0   r:   +test_pin_joint_interframe.<locals>.<lambda>D      xQTJr2   c                     > [        STT TS9$ Nr8   rw   r   r&   r/   r#   s   r0   r:   r   E      xQDIr2   c                     > [        STT TS9$ r   r   r   s   r0   r:   r   I  r   r2   c                     > [        STT TS9$ r   r   r   s   r0   r:   r   J  r   r2   r   rA   xyzr8   )rq   rr   rt   rs   g      r)      r   )rq   rr   rw   rs   r?   rv   )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   )	rD   rB   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                    ^^^^^^ [        S5      u  mn [        S5      u  mnmmmm[        STTTU TTTR                  S9nUR                  TR                  :X  d   eTR                  U5      [        [        T5      S[        T5      // SQ[        T5      S[        T5      * //5      :X  d   e[        S5      u  mnmmmm[        STTTU TTTR                  S9nUR                  TR                  :X  d   eTR                  U5      [        [        T5      * S[        T5      // SQ[        T5      S[        T5      //5      :X  d   e[        5       u  mnmm[        STTTU TR                  TR                  * TR                  S9nUR                  TR                  :X  d   eTR                  U5      [        [        T5      * [        T5      * S/[        T5      * [        T5      S// SQ/5      :X  d   e[        5       u  mnmm[        STTTU TR                  TR                  * TR                  S9nUR                  TR                  :X  d   eTR                  U5      [        / SQS[        T5      [        T5      /S[        T5      [        T5      * //5      :X  d   e[        S5      u  mnmmmm[        [        UUUU4S	 j5        [        [        UU4S
 j5        [        [        UU4S j5        [        [        UUUU4S j5        [        [        UUUU4S j5        [        S5      u  mnmmmm[        STTTTTR                  TR                  -   S9  [        [        UUUU4S j5        [        [        UUUU4S j5        g )Nru   Tr8   rt   rw   rs   r   r   r   )r   r   r   r   r   r   c            	      z   > [        STT [        T5      TR                  -  [        T5      TR                  -  -   S9$ Nr8   )rs   )r   r   r+   r   r,   r&   r!   r#   rD   s   r0   r:   +test_pin_joint_joint_axis.<locals>.<lambda>  s1    xQ36q6ACC<#a&133,3N Pr2   c                  0   > [        STT T R                  S9$ r   r   r+   r&   r#   s   r0   r:   r     s    xQaccBr2   c                  J   > [        STT TR                  T R                  -   S9$ r   r   r+   r,   r   s   r0   r:   r     s    xQaccACCiHr2   c            	      N   > [        STT TTTR                  T R                  -   S9$ Nr8   r   r   r&   r/   r#   r.   s   r0   r:   r     s$    xQTD66ACC< !r2   c            	      N   > [        STT TTTR                  TR                  -   S9$ r   r   r   s   r0   r:   r     s$    xQTD33< !r2   c            
      2   > [        STT TT[        S5      S9$ )Nr8   r   r   )r   r   r   s   r0   r:   r     s    hQTD!9r2   c            	      N   > [        STT TTTR                  TR                  -   S9$ r   )r   r,   r   s   r0   r:   r     s$    hQTD33<!r2   )r   r1   r   r,   rs   r   r	   r   r   r   r+   r   rX   	Exception)	rB   r"   pinr&   r/   r!   r#   r.   rD   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                  /   [        S5      u  p[        5       u  p#pE[        SXEUR                  * S9  UR                  * R	                  UR                  5      S:X  d   eUR                  R                  U5      * UR                  :X  d   eUR                  U5      [        / SQS[        U 5      * [        U 5      * /S[        U 5      * [        U 5      //5      :X  d   eUR                  U5      XR                  -  :X  d   eUR                  U5      R                  5       [        US-  5      :X  d   eUR                  R                  UR                  5      S:X  d   eUR                  R                  UR                  5      R                  U5      R                  5       S:X  d   eUR                  R!                  U5      S:X  d   e[        5       u  p#pE[        SXEUR"                  UR                  S9  UR"                  R	                  UR                  5      S:X  d   eUR"                  R                  U5      UR                  :X  d   eUR                  U5      [        S[        U 5      * [        U 5      * // SQS[        U 5      * [        U 5      //5      :X  d   eUR                  U5      XR                  -  :X  d   eUR                  U5      R                  U5      XR"                  -  :X  d   eUR                  U5      R                  5       [        US-  5      :X  d   eUR                  U5      R%                  UR"                  5      S:X  d   eUR                  R!                  U5      XR&                  -  :X  d   eUR                  R                  UR                  5      UR                  * :X  d   eUR                  R                  UR                  5      R                  U5      R                  5       [        U 5      UR"                  -  [        U 5      UR&                  -  -   :X  d   eUR                  R!                  U5      R	                  UR                  5      [(        S-  :X  d   e[        5       u  p#pE[        SXEUR"                  UR                  S	9  UR"                  R	                  UR                  5      S:X  d   eUR"                  R                  U5      UR                  :X  d   eUR                  U5      [        / S
Q[        U 5      * S[        U 5      /[        U 5      S[        U 5      //5      :X  d   eUR                  U5      XR"                  -  :X  d   eUR                  U5      R                  U5      XR                  -  :X  d   eUR                  U5      R                  5       [        US-  5      :X  d   eUR                  U5      R	                  UR                  5      nUR+                  US05      S:X  d   eUR                  R!                  U5      S:X  d   eUR                  R                  UR                  5      UR                  :X  d   e[        5       u  p#pE[        SXEUR                  UR                  UR                  UR"                  -   S9  [-        UR                  R	                  UR                  UR"                  -   5      5      S:X  d   eUR                  UR"                  -   R                  U5      R                  5       [        S5      UR                  -  :X  d   e[        UR                  U5      5      [        [        S5      S-  [        S5      * [        U 5      -  S-  [        S5      * [        U 5      -  S-  /[        S5      S-  [        S5      [        U 5      -  S-  [        S5      [        U 5      -  S-  /S[        U 5      * [        U 5      //5      :X  d   eUR                  U5      XR                  -  :X  d   eUR                  U5      R                  U5      R                  5       XR                  -  XR"                  -  -   [        S5      -  :X  d   eUR                  U5      R                  5       [        US-  5      :X  d   eUR                  U5      R	                  UR                  UR"                  -   5      nUR+                  US05      S:X  d   eUR                  R!                  U5      R                  5       XR&                  -  [        S5      -  :X  d   eUR                  R                  UR                  5      UR                  UR                  -
  :X  d   eUR                  R                  UR                  5      R                  U5      R                  5       S[        S5      S-  -
  UR                  -  [        S5      [        U 5      -  S-  UR"                  -  -   [        S5      [        U 5      -  S-  UR&                  -  -   :X  d   eUR                  R!                  U5      R                  U5      R                  5       [        S5      * U-  [        U 5      -  S-  UR"                  -  [        S5      U-  [        U 5      -  S-  UR&                  -  -   :X  d   eUR                  R!                  U5      R	                  UR                  5      [(        S-  :X  d   e[        5       u  p#pE[        SXEUR                  UR                  UR                  UR"                  -   UR&                  -
  S9  [-        UR                  R	                  UR                  UR"                  -   UR&                  -
  5      5      S:X  d   eUR                  UR"                  -   UR&                  -
  R                  U5      R                  5       [        S5      UR                  -  :X  d   e[        UR                  U5      5      [        [        S5      S-  [        S5      * [        U [(        S-  -   5      -  S-  [        S5      [        U [(        S-  -   5      -  S-  /[        S5      S-  [        S5      [        U [(        S-  -   5      -  S-  [        S5      [        U [(        S-  -   5      -  S-  /[        S5      * S-  [        S5      [        U S[(        -  S-  -   5      -  S-  [        S5      [        U S[(        -  S-  -   5      -  S-  //5      :X  d   eUR                  U5      XR                  -  :X  d   eUR                  U5      R                  U5      R                  5       XR                  -  XR"                  -  -   XR&                  -  -
  [        S5      -  :X  d   eUR                  U5      R                  5       [        US-  5      :X  d   eUR                  U5      R	                  UR                  UR"                  -   UR&                  -
  5      nUR+                  US05      R                  5       S:X  d   eUR                  R!                  U5      R                  5       XR"                  -  XR&                  -  -   [        S5      -  :X  d   eUR                  R                  UR                  5      UR                  UR                  -
  :X  d   eUR                  R                  UR                  5      R                  U5      R                  5       S[        S5      S-  -
  UR                  -  [        S5      [        U [(        S-  -   5      -  S-  UR"                  -  -   [        S5      [        U [(        S-  -   5      -  S-  UR&                  -  -
  :X  d   eUR                  R!                  U5      R                  U5      R                  5       [        S5      U-  [        U [(        S-  -   5      -  S-  UR"                  -  [        S5      U-  [        U [(        S-  -   5      -  S-  UR&                  -  -   :X  d   eUR                  R!                  U5      R	                  UR                  5      [(        S-  :X  d   e[        5       u  p#pE[/        S5      u  px[        SXEXrR                  -  XR                  -  UR                  UR"                  -   UR&                  -
  UR                  UR"                  -
  UR&                  -   S9  UR                  UR"                  -
  UR&                  -   R	                  UR                  UR"                  -   UR&                  -
  5      n[-        U5      S:X  d   eUR                  UR"                  -
  UR&                  -   R                  U5      R                  5       S[        U 5      -  S-  [1        S5      S-  -
  UR                  -  [1        S5      S-  S[        U [(        S-  -   5      -  S-  -
  UR"                  -  -   S[        U [(        S-  -   5      -  S-  [1        S5      S-  -
  UR&                  -  -   :X  d   e[        UR                  U5      5      [        [1        S5      S-  S[        U 5      -  S-  -
  S[        U [(        S-  -   5      -  S-  [1        S5      S-  -
  S[        U [(        S-  -   5      -  S-  [1        S5      S-  -   /S[        U [(        S-  -   5      -  S-  [1        S5      S-  -   S[        U 5      -  S-  [1        S5      S-  -
  S[        U [(        S-  -   5      -  S-  [1        S5      S-  -   /S[        U [(        S-  -   5      -  S-  [1        S5      S-  -
  S[        U [(        S-  -   5      -  S-  [1        S5      S-  -   S[        U 5      -  S-  [1        S5      S-  -
  //5      :X  d   eUR                  U5      XR                  -  XR"                  -  -
  XR&                  -  -   [        S5      -  -
  R                  5       (       d   eUR                  U5      R                  U5      R                  5       XR                  -  XR"                  -  -   XR&                  -  -
  [        S5      -  :X  d   eUR                  U5      R                  5       [        US-  5      :X  d   eUR                  U5      R	                  UR                  UR"                  -   UR&                  -
  5      nUR+                  US05      R                  5       S:X  d   eUR                  R!                  U5      R                  5       [        S5      U-  U-  S-  UR"                  -  [        S5      U-  U-  S-  UR&                  -  -   :X  d   eUR                  R                  UR                  5      XrR                  -  XR                  -  -
  :X  d   eUR                  R                  UR                  5      R                  U5      R                  5       XxS[        U 5      -  S-
  -  S-  -   UR                  -  US[        U [(        S-  -   5      -  S-   -  S-  UR"                  -  -   US[        U [(        S-  -   5      -  S-   -  S-  UR&                  -  -
  :X  d   eUR                  R!                  U5      R                  U5      R                  5       SU-  U-  [        U 5      -  S-  UR                  -  SU-  U-  [        U [(        S-  -   5      -  S-  UR"                  -  -   SU-  U-  [        U [(        S-  -   5      -  S-  UR&                  -  -   :X  d   eUR                  R!                  U5      R3                  UR                  UR"                  -
  UR&                  -   5      R                  5       S:X  d   eg )Nrl   r8   r   r   r   r)   r   r   rt   rq   r   r?   rq   rr   rw   rA   r   r         m nrq   rr   rw   rt   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)	rD   rB   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                  F   [        5       u  pp#[        R                  " X"R                  * UR                  5      nUR                  UR                  :X  d   e[        R                  " X"R
                  * UR
                  5      nUR                  UR                  :X  d   e[        R                  " X"R                  * UR                  5      nUR
                  UR
                  :X  d   e[        R                  " X"R                  * UR
                  -
  UR                  UR
                  -   5      nUR                  UR                  :X  d   e[        R                  " X"R
                  * UR                  -
  UR
                  UR                  -   5      nUR                  UR                  :X  d   e[        R                  " X"R                  * UR                  -
  UR                  UR                  -   5      nUR
                  UR
                  :X  d   e[        R                  " X"R                  * UR
                  -
  UR                  -
  UR                  UR
                  -   UR                  -   5      nUR
                  UR                  -
  UR
                  UR                  -
  :X  d   eg 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                  j  ^^^^	 [        S5      u  m	n [        S5      u  mnmmp#[        STTT	XUS9nUR                  UR                  :X  d   e[        S[        T	5      [        T	5      /S[        T	5      * [        T	5      // SQ/5      n[        S5      u  mnmmp#[        STTT	XUTR                  S9  TR                  U5      U:X  d   e[        S5      u  mnmmp#[        STTT	XUUR                  * S9  TR                  U5      U:X  d   e[        S5      u  mnmmp#[        [        UUUU	4S j5        g )	Nzq uTr8   )rt   rw   r   r   r   c                  6   > [        STT TTR                  -  S9$ r   )r   r   r   s   r0   r:   %test_pin_joint_axis.<locals>.<lambda>x  s    xQa!##gFr2   )r   r1   r   rs   r+   r	   r   r   r   r   r   rX   )
rB   r"   r.   r/   r8   N_R_Ar&   r!   r#   rD   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                    ^^^^^ [        5       u  mn mm[        STTTR                  TR                  -   S9nUR                  R
                  S:X  d   eUR                  R                  TR                  5      TR                  TR                  -   :X  d   eUR                  TR                  :X  d   e[        5       u  mn mmTR                  R                  STR                  TR                  -   5      n[        STTUTR                  S9nUR                  U:X  d   eUR                  TR                  :X  d   e[        5       u  mn mm[        [        UUU4S j5        [        S5      m[        5       u  mn mm[        [        UUUU4S j5        [        5       u  mn mmTR                  R                  STU R                  -  5      m[        [        UUU4S	 j5        [        S5      m[        [        UUU4S
 j5        g )Nr8   rq   	J_P_jointp)rq   rr   c                  N   > [        STT TR                  R                  T5      S9$ Nr8   r   )r   r+   	to_matrix)r&   r!   r#   s   r0   r:   'test_locate_joint_pos.<locals>.<lambda>  s    8CAACCMM!4DEr2   rD   c                  6   > [        STT TTR                  -  S9$ r   r   r   s   r0   r:   r    s    xQACCHr2   c                     > [        STT TS9$ Nr8   r   r   r&   r#   rr   s   r0   r:   r        xQ{Kr2   c                     > [        STT TS9$ r  r   r  s   r0   r:   r    r  r2   )r1   r   r,   r   rq   rx   r{   r|   rr   r   r   r<   r   rX   r   )r"   jointrq   r&   r!   r#   rr   rD   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                  Z  ^^^^ [        5       u  pmm[        S5      mTR                  X R                  S5        [	        STTTS9nUR
                  T:X  d   eUR
                  R                  U 5      S:X  d   eUR                  U:X  d   e[        S5      n[        5       u  pmm[        S5      mTR                  X R                  U5        [        [        UUU4S j5        [        5       u  pmm[        S5      mTR                  X R                  S5        [        [        UUU4S j5        g )	N	int_framer?   r8   r   r   rD   c                     > [        STT TS9$ r   r   )r&   r#   rt   s   r0   r:   )test_locate_joint_frame.<locals>.<lambda>  s    8CA9JKr2   c                     > [        STT TS9$ r   r   )r&   r#   rw   s   r0   r:   r    s    8CA8HIr2   )r1   r   r*   r   r   rt   r   rw   r   r   rX   )r!   r"   r  rD   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            
      H   [        5       u    pn[        S5      u  p4[        SX5      nUR                  S:X  d   eUR                  U:X  d   eUR
                  U:X  d   eUR                  [        U/5      :X  d   eUR                  [        U/5      :X  d   eUR                  [        XCR                  [        5      -
  /5      :X  d   eUR                  UR                  R                  :X  d   eUR                  R!                  UR"                  5      [%        S5      :X  d   eUR&                  R!                  UR"                  5      [%        S5      :X  d   eUR&                  R!                  UR                  5      U* UR                  R                  -  :X  d   eUR"                  R!                  UR"                  5      U* UR                  R                  -  :X  d   eUR"                  R)                  UR                  5      XAR                  R                  -  :X  d   eUR                  R+                  UR                  5      S:X  d   eUR                  R+                  UR                  5      S:X  d   eUR,                  " 5       S:X  d   e[        5       u  pgp[/        S5      u  p[1        S5      n
U
R3                  UR                  UR4                  [6        S-  5        [        SXXR                  R                  -  XR                  R4                  -  UR                  R8                  U
S9nUR                  UR                  R8                  :X  d   eUR                  R!                  UR"                  5      XR                  R4                  -  :X  d   eUR&                  R!                  UR"                  5      XR                  R                  -  :X  d   eUR&                  R!                  UR                  5      U* UR                  R8                  -  :X  d   eUR"                  R!                  UR"                  5      U* UR                  -  X6R8                  -  -
  XR4                  -  -   :X  d   eUR"                  R)                  UR                  5      XAR                  R8                  -  :X  d   eUR"                  R)                  U
5      [%        S5      :X  d   eUR                  R+                  UR                  5      S:X  d   eUR                  R+                  UR                  5      S:X  d   e[        5       u    pn[1        S5      n
U
R3                  UR                  UR4                  [6        S-  5        [        SXXR                  R8                  -  XR                  R                  -  UR                  R8                  U
S9nUR                  UR                  R8                  :X  d   eUR                  R!                  UR"                  5      XR                  R                  -  :X  d   eUR&                  R!                  UR"                  5      XR                  R8                  -  :X  d   eUR&                  R!                  UR                  5      U* UR                  R8                  -  :X  d   eUR"                  R!                  UR"                  5      U* U-
  UR                  R8                  -  XR                  R                  -  -   :X  d   eUR"                  R)                  UR                  5      XAR                  R8                  -  :X  d   eUR                  R+                  UR                  5      S:X  d   eUR                  R+                  UR                  5      S:X  d   eg )	Nq_S, u_Sr   r   z&PrismaticJoint: S  parent: P  child: Crk   r'   r)   rp   )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&   rD   rB   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                      [        S5      u  p[        5       u  p#pE[        SXEUR                  * S9  UR                  * R	                  UR                  5      S:X  d   eUR                  R                  U5      * UR                  :X  d   eUR                  U5      [        / SQ/ SQ/ SQ/5      :X  d   eUR                  R                  UR                  5      XR                  -  :X  d   eUR                  R                  UR                  5      R                  U5      R                  5       U * UR                  -  :X  d   eUR                  R                  U5      XR                  -  :X  d   eUR                  R                  U5      R                  U5      U* UR                  -  :X  d   eUR                  U5      S:X  d   eUR                  U5      S:X  d   e[        5       u  p#pE[        SXEUR                  UR                  S9  UR                  R	                  UR                  5      S:X  d   eUR                  R                  U5      UR                  :X  d   eUR                  U5      [        / SQ/ S	Q/ SQ/5      :X  d   eUR                  R                  U5      XR                  -  :X  d   eUR                  R                  U5      R                  U5      XR                  -  :X  d   eUR                  R                  UR                  5      XR                  -  UR                  -
  :X  d   eUR                  R                  UR                  5      R                  U5      R                  5       XR                  -  UR                  -   :X  d   eUR                  U5      S:X  d   eUR                  U5      S:X  d   e[        5       u  p#pE[        SXEUR                  UR                  S
9  UR                  R	                  UR                  5      S:X  d   eUR                  R                  U5      UR                  :X  d   eUR                  U5      [        / SQ/ SQ/ SQ/5      :X  d   eUR                  R                  U5      XR                  -  :X  d   eUR                  R                  U5      R                  U5      XR                  -  :X  d   eUR                  R                  UR                  5      UR                  XR                  -  -   :X  d   eUR                  U5      S:X  d   eUR                  U5      S:X  d   e[        5       u  p#pE[        SXEUR                  UR                  UR                  UR                  -   S9  UR                  R	                  UR                  UR                  -   5      S:X  d   eUR                  UR                  -   R                  U5      [        S5      UR                  -  :X  d   eUR                  U5      [        [        S5      S-  [        S5      * S-  S/[        S5      S-  [        S5      S-  S// SQ/5      :X  d   eUR                  R                  UR                  5      U S-   UR                  -  UR                  -
  :X  d   eUR                  R                  UR                  5      R                  U5      U [        S5      S-  -
  S-   UR                  -  [        S5      S-  UR                  -  -   :X  d   eUR                  R                  U5      R                  U5      XR                  UR                  -   -  [        S5      -  :X  d   eUR                  R                  U5      XR                  -  :X  d   eUR                  U5      S:X  d   eUR                  U5      S:X  d   e[        5       u  p#pE[        SXEUR                  UR                  UR                  UR                  -   UR                  -
  S9  UR                  R	                  UR                  UR                  -   UR                  -
  5      R                  5       S:X  d   eUR                  UR                  -   UR                  -
  R                  U5      [        S5      UR                  -  -
  R                  5       S:X  d   e[        UR                  U5      5      [        [        S5      S-  [        S5      * S-  [        S5      S-  /[        S5      S-  [        S5      S-  [!        S5      S-  -   [!        S5      S-  [        S5      S-  -
  /[        S5      * S-  [!        S5      S-  [        S5      S-  -
  [        S5      S-  [!        S5      S-  -   //5      :X  d   eUR                  R                  UR                  5      U S-   UR                  -  UR                  -
  :X  d   eUR                  R                  UR                  5      R                  U5      U [        S5      S-  -
  S-   UR                  -  [        S5      S-  UR                  -  -   [        S5      S-  UR                  -  -
  -
  R                  5       S:X  d   eUR                  R                  U5      XR                  -  :X  d   eUR                  R                  U5      R                  U5      [        S5      U-  S-  UR                  -  [        S5      U-  S-  UR                  -  -   [        S5      U-  S-  UR                  -  -
  -
  R                  5       (       d   eUR                  U5      S:X  d   eUR                  U5      S:X  d   e[        5       u  p#pE[#        S5      u  pg[        SXEXbR                  -  XsR                  -  UR                  UR                  -   UR                  -
  UR                  UR                  -
  UR                  -   S9  UR                  UR                  -
  UR                  -   R	                  UR                  UR                  -   UR                  -
  5      R                  5       S:X  d   eUR                  UR                  -   UR                  -
  R                  U5      UR                  UR                  -
  UR                  -   -
  R                  5       S:X  d   e[        UR                  U5      5      [        [!        S5      * S-  [!        S5      * S-  [!        S5      S-  /[!        S5      S-  [!        S5      S-  [!        S5      S-  /[!        S5      * S-  [!        S5      S-  [!        S5      S-  //5      :X  d   eUR                  R                  UR                  5      U[        S5      U -  S-  -   UR                  -  [        S5      U -  S-  UR                  -  -
  [        S5      U -  S-  UR                  -  -   XsR                  -  -
  -
  R                  U5      R                  5       S:X  d   eUR                  R                  UR                  5      R                  U5      XgS-  -   [        S5      U -  S-  -   UR                  -  SU-  S-  [        S5      U -  S-  -
  UR                  -  -   SU-  S-  [        S5      U -  S-  -   UR                  -  -   -
  R                  5       S:X  d   eUR                  R                  U5      R                  U5      [        S5      U-  S-  UR                  -  [        S5      U-  S-  UR                  -  -
  [        S5      U-  S-  UR                  -  -   -
  R                  5       S:X  d   eUR                  R                  U5      R                  U5      [        S5      U-  S-  UR                  -  [        S5      U-  S-  UR                  -  -   [        S5      U-  S-  UR                  -  -
  -
  R                  5       S:X  d   eUR                  U5      S:X  d   eUR                  U5      S:X  d   eg )Nr  r   r   r   r   r   r   r   r   r   r   r   r)   r?   rA   r   r   r   r   )r   r1   r   r+   r   r   r   r	   r|   r{   r
   r   r   r,   r   r   r   r   )rD   rB   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                  
   [        5       u  pp#[        S5      u  pEpg[        SX#5      nUR                  S:X  d   eUR                  U:X  d   eUR
                  U:X  d   eUR                  [        XE/5      :X  d   eUR                  [        Xg/5      :X  d   eUR                  U:X  d   eUR                  U:X  d   eUR                  U:X  d   eUR                  U:X  d   eUR                  [        XdR                  [        5      -
  XuR                  [        5      -
  /5      :X  d   eUR                   U R"                  :X  d   eUR$                  R'                  UR(                  5      [+        S5      :X  d   eUR,                  R'                  UR(                  5      [+        S5      :X  d   eUR,                  R'                  UR.                  5      U* U R"                  -  :X  d   eUR(                  R'                  UR(                  5      XPR"                  -  :X  d   eUR$                  R1                  U 5      XpR"                  -  :X  d   eUR3                  U 5      X`R"                  -  :X  d   eUR4                  U :X  d   eUR6                  U:X  d   eUR9                  5       S:X  d   e[        S5      u  pp[;        S5      u  p[        S5      u  pp#nn[        SX#XXU R"                  -  XR<                  -  UUSU R>                  -  S	9nUR                  [        X/5      :X  d   eUR                  [        X/5      :X  d   eUR                  U	:X  d   eUR                  U:X  d   eUR                  U:X  d   eUR                  U:X  d   eUR                  [        XR                  [        5      -
  XR                  [        5      -
  /5      :X  d   eUR                   SU R>                  -  :X  d   eUR$                  R'                  UR(                  5      XR<                  -  :X  d   eUR,                  R'                  UR(                  5      XR"                  -  :X  d   eUR,                  R'                  UR.                  5      U* U R>                  -  :X  d   eUR(                  R'                  UR(                  5      XR"                  -  XPR>                  -  -   XR<                  -  -
  :X  d   eUR(                  R1                  U 5      XR>                  -  X-  UR>                  -  -
  :X  d   eUR3                  U 5      XR>                  -  :X  d   eg )
Nzq0:2_J, u0:2_Jr8   r   z(CylindricalJoint: J  parent: P  child: Cz
q0:2, u0:2l, mTr)   )rotation_coordinaterotation_speedtranslation_speedrq   rr   rt   rw   rs   ) 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CjrI   rJ   rN   rO   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                     [        5       u  pp#[        S5      u  pEn[        S5      u  pxn	[        SX#5      n
U
R                  S:X  d   eU
R                  U:X  d   eU
R
                  U:X  d   eU
R                  [        XEU/5      :X  d   eU
R                  [        XxU	/5      :X  d   eU
R                  U:X  d   eU
R                  [        XV/5      :X  d   eU
R                  U:X  d   eU
R                  [        X/5      :X  d   eU
R                  [        XtR                  [        5      -
  XR                  [        5      -
  XR                  [        5      -
  /5      :X  d   eU
R                   U R"                  :X  d   eU
R$                  U R&                  U R(                  /:X  d   eU
R*                  R-                  UR.                  5      [1        S5      :X  d   eU
R2                  R-                  UR.                  5      [1        S5      :X  d   eXPR&                  -  X`R(                  -  -   nU
R2                  R-                  U
R*                  5      U* :X  d   eUR.                  R-                  UR.                  5      U:X  d   eU
R*                  R5                  U 5      XR&                  -  XR(                  -  -   :X  d   eUR7                  U 5      XpR"                  -  :X  d   eU
R8                  U :X  d   eU
R:                  U:X  d   eU
R=                  5       S:X  d   e[        S5      u  ppnn[?        S5      u  nn[        S5      u  pp#nn[        SX#UX/UU/UU R"                  -  UUR&                  -  UUS	9
n
U
R                  [        XU/5      :X  d   eU
R                  [        UUU/5      :X  d   eU
R                  U:X  d   eU
R                  [        X/5      :X  d   eU
R                  U:X  d   eU
R                  [        UU/5      :X  d   eU
R                  [        X|R                  [        5      -
  UUR                  [        5      -
  UUR                  [        5      -
  /5      :X  d   eU
R                   UR"                  :X  d   eU
R$                  UR&                  UR(                  /:X  d   eU
R*                  R-                  UR.                  5      UUR&                  -  :X  d   eU
R2                  R-                  UR.                  5      UU R"                  -  :X  d   eU
R2                  R-                  U
R*                  5      XR&                  -  XR(                  -  -   :X  d   eUR.                  R-                  UR.                  5      UU R"                  -  XR&                  -  -
  XR(                  -  -
  UUR&                  -  -
  :X  d   eUR.                  R5                  U 5      U* U R&                  -  UU R(                  -  -
  UU-  UR"                  -  -   :X  d   eUR7                  U 5      XpR"                  -  :X  d   eg )
Nzq0:3_Jzu0:3_Jr8   r   z#PlanarJoint: J  parent: P  child: C
q0:3, u0:3r  T)r  planar_coordinatesplanar_speedsrq   rr   rt   rw   ) 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_CrI   rJ   rd   rN   rO   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                  .   [        S5      u  pp#pE[        S5      u  pg[        5       u  pp[        SXXU/X4U/XhR                  -  U* UR                  -  UR                  UR
                  [        S5      -  -   U	R                  U	R
                  [        S5      -  -   S9nUR                  R                  U5      UR                  UR
                  [        S5      -  -   R                  5       :X  d   eUR                  R                  U	5      U	R                  U	R
                  [        S5      -  -   R                  5       :X  d   eUR                  R                  UR                  5      [        S-  :X  d   eUR                  U	5      R                  U SUSUS05      [        S5      :X  d   e[        [!        U 5      [        S5      * [#        U 5      -  S-  [#        U 5      S-  /[        S5      [#        U 5      -  S-  S[!        U 5      -  S	-  S
-   [        S5      S[!        U 5      -
  -  S	-  /[#        U 5      * S-  [        S5      S[!        U 5      -
  -  S	-  [!        U 5      S	-  S-   //5      n[%        UR                  U	5      U-
  5      ['        S5      :X  d   eg )Nr"  zl1:3r8   rA   )rq   rr   rt   rw   r   r   r)   r   g      ?r?   g      ?)r   r   r1   r   r   r,   r   r%  r   	normalizer   r   r   r   r   r	   r   r   r
   r   )rI   rJ   rd   rN   rO   rf   l1l2r!   r"   r#   r&   r8   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            	          [        5       u  pp#[        S5      u  pEpgp[        SX#5      n
U
R                  S:X  d   eU
R                  U:X  d   eU
R
                  U:X  d   eU
R                  [        XEU/5      :X  d   eU
R                  [        XxU	/5      :X  d   eU
R                  [        XtR                  [        5      -
  XR                  [        5      -
  XR                  [        5      -
  /5      :X  d   eU
R                  R                  UR                  5      [        S5      :X  d   eU
R                   R                  UR                  5      [        S5      :X  d   eU
R                   R                  U
R                  5      [        S5      :X  d   eUR                  R                  UR                  5      [        S5      :X  d   eUR                  R#                  U 5      [        S5      :X  d   eU R%                  U5      U* ['        U5      -  ['        U5      -  U[)        U5      -  -
  UR*                  -  U[)        U5      -  ['        U5      -  U['        U5      -  -
  UR,                  -  -   U* [)        U5      -  U	-
  UR.                  -  -   :X  d   eUR%                  U 5      U['        U5      -  ['        U5      -  U[)        U5      -  -   UR*                  -  U* [)        U5      -  ['        U5      -  U['        U5      -  -   UR,                  -  -   U[)        U5      -  U	-   UR.                  -  -   :X  d   eU
R0                  " 5       S:X  d   eU
R2                  S:X  d   eU
R4                  S:X  d   eU
R6                  b   eg )Nq0:3_S, u0:3_Sr   r   z&SphericalJoint: S  parent: P  child: CBODY{   )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&   rI   rJ   rd   rN   rO   rf   r   s              r0   test_spherical_jointr7    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            	      l   [        S5      u  pn[        SS5      u  p4n[        5       u  pgp[        SXXU/X4U/S9n
U
R                  [	        XU/5      :X  d   eU
R
                  [	        X4U/5      :X  d   eU
R                  [	        / SQ5      :X  d   eUR                  U5      U* [        U5      -  [        U5      -  U[        U5      -  -
  UR                  -  U[        U5      -  [        U5      -  U[        U5      -  -
  UR                  -  -   U* [        U5      -  U-
  UR                  -  -   :X  d   eg )Nzq0:3r?   r   )ry   rW   )r   r   r   )r   r1   r   ry   r	   rW   rz   r   r   r   r+   r,   r   )rI   rJ   rd   rN   rO   rf   r!   r"   r#   r&   r   s              r0   /test_spherical_joint_speeds_as_derivative_termsr9    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                    ^	^
^^^^^^^^ [        S5      u  pp#pE[        S5      u  mmmmmmmm[        5       u  pgm
m	[        ST
T	TTT/TTT/5      nUR                  [	        TTT/5      :X  d   eUR
                  [	        TTT/5      :X  d   e[        5       u  pgm
m	[        ST
T	[	        TTT/5      [	        TTT/5      5      nUR                  [	        TTT/5      :X  d   eUR
                  [	        TTT/5      :X  d   e[        5       u  pgm
m	[        [        U	U
UUU4S j5        [        [        U	U
UUUUUUU4	S j5        [        [        U	U
UUUUUUU4	S j5        g )Nr1  z
q0:4, u0:4r   c            	      L   > [        STT [        TT/5      [        T/5      5      $ Nr   r   r	   )r&   r#   rI   rJ   rN   s   r0   r:   -test_spherical_joint_coords.<locals>.<lambda>	  s!    >#q!VRH-=vrd|Lr2   c                  T   >	 [        STT [        TTTT/5      [        TTT/5      5      $ r<  r=  )	r&   r#   rI   rJ   rd   re   rN   rO   rf   s	   r0   r:   r>    s/    ~Q62r2r*+VRRL-A Cr2   c                  T   >	 [        STT [        TTT/5      [        TTTT/5      5      $ r<  r=  )	r&   r#   rI   rJ   rd   rN   rO   rf   u4s	   r0   r:   r>    s/    ~Q62r2,'RR0@)A Cr2   )r   r1   r   ry   r	   rW   r   rX   )q0sq1sq2su0su1su2sr!   r"   r   r&   r#   rI   rJ   rd   re   rN   rO   rf   rA  s            @@@@@@@@@@r0   test_spherical_joint_coordsrH    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                  l  ^^^^^^ [        S5      u  mmmmmm[        [        T5      * [        T5      * [        T5      -  [        T5      [        T5      -  /[        T5      * [        T5      -  [        T5      [        T5      -  [        T5      -  [        T5      [        T5      -  -
  [        T5      * [        T5      -  [        T5      -  [        T5      [        T5      -  -
  /[        T5      [        T5      -  [        T5      * [        T5      -  [        T5      [        T5      -  [        T5      -  -
  [        T5      * [        T5      -  [        T5      [        T5      -  [        T5      -  -   //5      n [        T* [        T5      -  T-
  /T* [        T5      -  [        T5      -  T[        T5      -  -   /T[        T5      -  [        T5      -  T[        T5      -  -   //5      n[        [	        S5      * T[        T[
        S-  -   5      -  [        T5      -  T[        T[
        S-  -   5      -  -   -  /T* [        T5      -  T-
  /T* [        T5      -  T-
  //5      n[        S5      u  p4pVpx[        SXVTTT/TTT/UR                  UR                  -   UR                  * UR                  -   XxSSS9n	U	R                  R                  5       S	:X  d   eU	R                  S:X  d   e[        UR                  U5      U -
  5      [!        S
5      :X  d   e[        UR#                  U5      R%                  U5      U-
  5      [!        S
S5      :X  d   e[        UR&                  R)                  U5      R%                  U5      5      U:X  d   e[        S5      u  p4pVpx[        SXVTTT/TTT/UR                  UR                  -   UR                  * UR                  -   XxS	TTT4SS9n	UUUUUU4S jn
U	R                  R                  5       S	:X  d   eU	R                  S:X  d   e[        UR                  U5      U
" U 5      -
  5      [!        S
5      :X  d   e[        UR#                  U5      R%                  U5      U
" U5      -
  5      [!        S
S5      :X  d   e[        UR&                  R)                  U5      R%                  U5      5      U
" U5      :X  d   e[        S5      u  p4pVpx[        SXVTTT/TTT/UR                  UR                  -   UR                  * UR                  -   XxSSS9n	U	R                  R                  5       S	:X  d   eU	R                  S:X  d   e[        UR                  U5      [        [        T5      * [        T5      -  [        T5      [        T5      -  [        T5      -  [        T5      [        T5      -  -
  [        T5      [        T5      -  [        T5      -  [        T5      [        T5      -  -   /[        T5      * [        T5      * [        T5      -  [        T5      * [        T5      -  /[        T5      [        T5      -  [        T5      * [        T5      -  [        T5      [        T5      -  [        T5      -  -
  [        T5      [        T5      -  [        T5      [        T5      -  [        T5      -  -
  //5      -
  5      [!        S
5      :X  d   e[        UR#                  U5      R%                  U5      [        T[        T5      -  T-
  /T[        T5      -  [        T5      -  T[        T5      -  -
  /T[        T5      -  [        T5      -  T[        T5      -  -   //5      -
  5      [!        S
S5      :X  d   e[        UR&                  R)                  U5      R%                  U5      5      [        [	        S5      * T[        T[
        S-  -   5      -  [        T5      -  T[        T[
        S-  -   5      -  -   -  /T[        T5      -  T-
  /T[        T5      -  T-
  //5      :X  d   eg )Nr"  r)   r   Tr   bodyr3  ry   rW   rq   rr   rt   rw   rot_type	rot_orderr2  rA   r?   	ry   rW   rq   rr   rt   rw   rL  amountsrM  c                 >   > U R                  TTTTTTTTTTTT05      $ r   r   exprrI   rJ   rd   rN   rO   rf   s    r0   r:   2test_spherical_joint_orient_body.<locals>.<lambda>0  *    	RRRRRR8!:r2   BodYyxz)r   r	   r   r   r   r   r1   r   r+   r,   r   r4  upperr5  r
   r   r   r   r   r|   r   r   N_w_AN_v_Cor!   r"   r#   r&   r.   r/   r   switch_orderrI   rJ   rd   rN   rO   rf   s              @@@@@@r0    test_spherical_joint_orient_bodyr]    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                    ^^^^^^ [        S5      u  mmmmmm[        [        T5      * [        T5      -  [        T5      [        T5      -  [        T5      -  -
  [        T5      [        T5      -  [        T5      -  [        T5      [        T5      -  -
  [        T5      [        T5      -  /[        T5      * [        T5      -  [        T5      [        T5      -  [        T5      -  -   [        T5      * [        T5      -  [        T5      -  [        T5      [        T5      -  -
  [        T5      * [        T5      -  /[        T5      [        T5      -  [        T5      * [        T5      -  [        T5      //5      n [        T[        T5      -  T[        T5      -  [        T5      -  -
  /T[        T5      -  T[        T5      -  [        T5      -  -   /TT[        T5      -  -
  //5      n[        TT[        T5      -  -
  /TT[        T5      -  -
  /[	        S5      T* [        T[
        S-  -   5      -  T[        T[
        S-  -   5      -  [        T5      -  -   -  //5      n[        S5      u  p4pVpx[        SXVTTT/TTT/UR                  UR                  -   UR                  * UR                  -   XxSSS9n	U	R                  R                  5       S	:X  d   eU	R                  S:X  d   e[        UR                  U5      U -
  5      [!        S
5      :X  d   e[        UR#                  U5      R%                  U5      5      U:X  d   e[        UR&                  R)                  U5      R%                  U5      5      U:X  d   eUUUUUU4S jn
[        S5      u  p4pVpx[        SXVTTT/TTT/UR                  UR                  -   UR                  * UR                  -   XxS	TTT4SS9n	U	R                  R                  5       S	:X  d   eU	R                  S:X  d   e[        UR                  U5      U
" U 5      -
  5      [!        S
5      :X  d   e[        UR#                  U5      R%                  U5      5      U
" U5      :X  d   e[        UR&                  R)                  U5      R%                  U5      5      U
" U5      :X  d   e[        S5      u  p4pVpx[        SXVTTT/TTT/UR                  UR                  -   UR                  * UR                  -   XxSSS9n	U	R                  R                  5       S	:X  d   eU	R                  S:X  d   e[        UR                  U5      [        [        T5      * [        T5      -  [        T5      * [        T5      -  [        T5      [        T5      -  [        T5      -  -   [        T5      [        T5      -  [        T5      -  [        T5      [        T5      -  -   /[        T5      * [        T5      * [        T5      -  [        T5      * [        T5      -  /[        T5      [        T5      -  [        T5      * [        T5      -  [        T5      [        T5      -  [        T5      -  -
  [        T5      * [        T5      -  [        T5      -  [        T5      [        T5      -  -   //5      -
  5      (       d   e[        UR#                  U5      R%                  U5      [        T* T[        T5      -  -   /T* [        T5      -  T[        T5      -  [        T5      -  -   /T[        T5      -  T[        T5      -  [        T5      -  -   //5      -
  5      [!        S
S5      :X  d   e[        UR&                  R)                  U5      R%                  U5      [        T[        T5      -  T[        T5      -  [        T5      -  -   /T[        T5      -  T[        T5      -  [        T5      -  -   /TT[        T5      -  -   T[        T5      -  -
  T[        T5      -  [        T5      -  -
  //5      -
  5      [!        S
S5      :X  d   eg )Nr"  r)   r   Tr   spacer3  rK  SPACErA   c                 >   > U R                  TTTTTTTTTTTT05      $ r   rQ  rR  s    r0   r:   3test_spherical_joint_orient_space.<locals>.<lambda>h  rU  r2   rN  SPaCezxyr?   )r   r	   r   r   r   r   r1   r   r+   r   r,   r4  rX  r5  r
   r   r   r   r   r|   r   rY  s              @@@@@@r0   !test_spherical_joint_orient_spacere  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            	      	   [        5       u    pn[        SX5      nUR                  S:X  d   eUR                  U:X  d   eUR                  U:X  d   eUR
                  [        5       :X  d   eUR                  [        5       :X  d   eUR                  [        SS/ 5      R                  :X  d   eUR                  R                  UR                  5      [        S5      :X  d   eUR                  R                  UR                  5      [!        S5      :X  d   eUR"                  R                  UR                  5      [!        S5      :X  d   eUR"                  R                  UR                  5      [!        S5      :X  d   eUR                  R                  UR                  5      [!        S5      :X  d   eUR                  R%                  UR                  5      [!        S5      :X  d   eUR                  R'                  UR                  5      S:X  d   eUR                  R'                  UR                  5      S:X  d   eUR)                  5       S:X  d   e[        5       u  pEp[+        S5      u  pg[-        S5      nUR/                  UR                  UR0                  [2        S-  5        [        SXXaR                  R4                  -  XrR                  R0                  -  US	9nUR                  R                  UR                  5      XrR                  R0                  -  :X  d   eUR"                  R                  UR                  5      XaR                  R4                  -  :X  d   eUR"                  R                  UR                  5      [!        S5      :X  d   eUR                  R                  UR                  5      U* UR4                  -  XuR0                  -  -   :X  d   eUR                  R%                  UR                  5      [!        S5      :X  d   eUR                  R%                  U5      [!        S5      :X  d   eUR                  R'                  UR                  5      S:X  d   eUR                  R'                  UR                  5      S:X  d   eUR4                  UR6                  :X  d   e[9        5          [;        X5        S S S 5        g ! , (       d  f       g = f)
NWr?   r   rA   z!WeldJoint: W  parent: P  child: Crk   r'   r)   )rq   rr   rt   )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&   rg  r!   r"   r   r   r.   s	            r0   test_weld_jointri    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   4S		
Sc            
      B   [        S5      u  p[        5       u  p#pE[        5          [        SXEUR                  * S9  S S S 5        UR                  * R                  UR                  5      S:X  d   eUR                  R                  U5      * UR                  :X  d   eUR                  U5      [        / SQS[        U 5      * [        U 5      * /S[        U 5      * [        U 5      //5      :X  d   eUR                  U5      XR                  -  :X  d   eUR                  U5      R                  5       [        US-  5      :X  d   e[        5       u  p#pE[        5          [        SXEUR                  UR                  -   S9  S S S 5        UR                  R                  UR                  UR                  -   5      S:X  d   eUR                  R                  U5      UR                  UR                  -   [        S5      -  :X  d   eUR                  U5      [        [        S5      S-  [        S5      S-  S/[        S5      * S-  [        S5      S-  S// SQ/5      :X  d   eUR                  U5      [!        S5      :X  d   eg ! , (       d  f       GNH= f! , (       d  f       GN= f)	Nrl   r8   )
child_axisr   r   r)   )parent_axisr   )r   r1   r   r   r+   r   r   r   r	   r   r   r   r   r   r   r,   r   )rD   rB   r!   r"   r#   r&   s         r0   !test_deprecated_parent_child_axisrm    s&   *%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   I=#J=
J
Jc            	         [        5       u  pp#[        5          [        SX#U R                  U R                  -   UR                  UR
                  -
  S9nS S S 5        WR                  R                  UR                  5      U R                  U R                  -   :X  d   eUR                  R                  UR                  5      UR                  UR
                  -
  :X  d   e[        5       u  pp#[        5          [        SX#U R
                  U R                  -   UR                  UR                  -
  S9nS S S 5        WR                  R                  UR                  5      U R
                  U R                  -   :X  d   eUR                  R                  UR                  5      UR                  UR                  -
  :X  d   eg ! , (       d  f       GNr= f! , (       d  f       N= f)Nr8   )parent_joint_poschild_joint_pos)r1   r   r   r+   r,   r   rq   r{   r|   rr   r   )r!   r"   r#   r&   r   sliders         r0   test_deprecated_joint_posrr    sV   !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   ;F>6;G>
G
G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/  r7  r9  rH  r]  re  ri  rm  rr   r2   r0   <module>r     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   