
    sgB                        d dl mZ d dlmZ d dlmZ d dlmZ d dlm	Z	m
Z
 d dlmZmZmZmZ d dlmZ d dlmZmZmZmZmZmZmZmZmZmZmZ d d	lmZ d d
l m!Z!m"Z"m#Z# d dl$m%Z%m&Z& ejN                  Z(d)dZ)d Z*d Z+d Z,d Z-d Z.d Z/d Z0d Z1d Z2d Z3d Z4d Z5d Z6d Z7d Z8d Z9d Z:d Z;d Z<d  Z=d! Z>d" Z?d# Z@d$ ZAd% ZBd& ZCd' ZDy()*    )
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                 :   t        d      }t        d      }t        d|      }t        d|      }| rdt        d      t        d      }}|j                  ||j                  t               |j                  ||j
                  t         dz         ||||||fS ||||fS )	NNAPframeCP_intC_int   )r   r   orient_axisxr   y)
interframer!   r"   r#   r&   PintCints          [/var/www/html/venv/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_joint.py_generate_bodyr1      s    sAsA#QA#QA#G,nW.EdACC$ACC"q)!Q4%%aA:    c                  \     t        d      t        d       t        t         fd       y )Nparentchildc                      t        d       S NJr   r5   r4   s   r0   <lambda>ztest_Joint.<locals>.<lambda>#   s    eC7 r2   )r   r   	TypeErrorr9   s   @@r0   
test_Jointr<       s"    x FgE
978r2   c            	         t        d      \  } }}}t        d      \  }}}}}}	}
t        d      \  }}}}t               \  }}t        d      j                  | d      t	        | g      k(  sJ j                  |gd      t	        |g      k(  sJ j                  |gdd      t	        |g      k(  sJ j                  d d      t	        |g      k(  sJ j                  d gd      t	        |g      k(  sJ j                  d d gd      t	        |g      k(  sJ j                  d d      t	        ||g      k(  sJ j                  g d      t	        ||g      k(  sJ j                  g dd      t	        ||g      k(  sJ j                  d |gdd      t	        ||g      k(  sJ j                  d dd      t	        ||g      k(  sJ j                  d dd	      t	        |g      k(  sJ j                  g dd	      t	        |||	g      k(  sJ j                  d dd
      t	        |g      k(  sJ j                  g dd	dd
      t	        |	g      k(  sJ j                  g dd      t	        ||g      k(  sJ t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t               \  }}t        dj                  t                     t               \  }}t        t        fd       t        t        fd       y )Nzq u q_J u_Jzq0:4_J u0:4_Jz	q0:4 u0:4r8      r)   )offset   uT)number_singleqc                  ,     j                  gd      S )Nr>   _fill_coordinate_listr8   q0q1s   r0   r:   z,test_coordinate_generation.<locals>.<lambda>F       q66BxC r2   c                  0     j                  d gdd      S )Nr)   rA   rE   )r8   u0u1s   r0   r:   z,test_coordinate_generation.<locals>.<lambda>G   s    q66B~q#N r2   c                  ,     j                  gd      S )Nr@   rE   rG   s   r0   r:   z,test_coordinate_generation.<locals>.<lambda>H   rJ   r2   c                  >     j                  t        d      gd      S )NrI   r)   )rF   r   )r8   rH   s   r0   r:   z,test_coordinate_generation.<locals>.<lambda>J   s    a55r74=6I1M r2   c                  2     j                  z   gd      S )Nr)   rE   rG   s   r0   r:   z,test_coordinate_generation.<locals>.<lambda>K   s    a55rBwmQG r2   c                  &    t        d d d g      S r7   r   )r&   r#   q1js   r0   r:   z,test_coordinate_generation.<locals>.<lambda>Q   s    ~c1a#tT9JK r2   c                  (    t        d g      S )Nr8   )speedsrR   )r&   r#   rL   rM   s   r0   r:   z,test_coordinate_generation.<locals>.<lambda>R   s    ~c1aRM r2   )
r   r1   r   rF   r	   r   
ValueErrorr;   difft)rC   rA   qjujq0jq2jq3ju0ju1ju2ju3jq2q3u2u3_r&   r8   r#   rH   rI   rS   rL   rM   s                   @@@@@@@@r0   test_coordinate_generationrg   &   sq   !-0LAq"b-;O-L*Cc3S#s%3K%@"BBBB!JAq!QaA""1a(FA3K777""A3*faSk999""A3!"4sCCC""4+vrd|;;;""D61-===""Bd#3Q76	S#<    ""4+vsCo/FFFF""2q)VS#sO-DDDD""2q"3vsCo7NNNN""Bb>1Q"?6	S"D   ""41"5c
9KKKK""4C0FB4L@@@""2q#.&#sC2IIII""4$"?63%=PPP""2q#q$763%=HHH""2q#.&#sC2IIII
:CD
:NO
:CD
9MN
9GH!JAq!QS!QBGGAJ'!JAq!Q
:KL
:MNr2   c                  @   t        d      } t        d      }t        d      \  }}t        d      \  }}t        d| |      }|j                  dk(  sJ |j
                  | k(  sJ |j                  |k(  sJ |j                  t        |g      k(  sJ |j                  t        |g      k(  sJ |j                  t        ||j                  t              z
  g      k(  sJ |j                  | j                  j                  k(  sJ |j                   j#                  |j$                        t'        d      k(  sJ |j(                  j#                  | j$                        t'        d      k(  sJ |j(                  j#                  |j*                        t'        d      k(  sJ |j$                  j#                  | j$                        t'        d      k(  sJ |j,                  | j                  k(  sJ |j.                  |j                  k(  sJ |j1                         dk(  sJ t        d      }t        d	      }t3        d
      }	|	j5                  |j                  |j6                  t8        dz         t        d||||j                  j                  z  ||j                  j6                  z  |j                  j:                  |	      }
|
j<                  |j                  j:                  k(  sJ |
j*                  j#                  |j$                        ||j                  j6                  z  k(  sJ |
j>                  j#                  |j$                        ||j                  j                  z  k(  sJ |
j>                  j#                  |
j*                        t'        d      k(  sJ |j$                  j#                  |j$                        | |j                  j                  z  ||j                  j6                  z  z   k(  sJ |
j,                  |	k(  sJ |
j.                  |j                  k(  sJ t        d      \  }}tA        d      \  }}} }}	}| j$                  jC                  d|j                  |j6                  z         }|j$                  jC                  d|j6                  |j:                  z         }t        d| ||||||	||j:                  
      }|j                  |j:                  k(  sJ |j(                  jE                  |      dk(  sJ |j(                  |k(  sJ |j                   |k(  sJ |j                   j#                  | j$                        |j                  |j6                  z   k(  sJ |j(                  j#                  |j$                        |j6                  |j:                  z   k(  sJ |j$                  j#                  | j$                        |j                  |j6                  z   |j6                  z
  |j:                  z
  k(  sJ |j$                  jE                  |      jG                  |      |tI        |      z  |tK        |      z  z
  |j                  z  | tI        |      z  |tK        |      z  z
  |j6                  z  z   k(  sJ |j,                  |	k(  sJ |j.                  |k(  sJ y )Nr#   r&   l mq_J, u_Jr8   r   z PinJoint: J  parent: P  child: CP1C1r'   r)   J1parent_pointchild_point
joint_axisparent_interframeq, uTro   rp   ro   rp   rr   child_interframerq   )&r   r   r   r   namer4   r5   coordinatesr	   rU   kdesrW   rX   rq   r%   r+   rp   pos_from
masscenterr   ro   _child_pointrr   ru   __str__r   r*   r,   r   z_joint_axis_parent_pointr1   	locatenewvelexpressr   r   )r#   r&   lmrC   rA   Pjrk   rl   r.   rm   r!   r"   r/   ro   rp   r8   s                    r0   test_pin_jointr   U   sX   #A#A5>DAq*%DAq	#q!	B77c>>99>>88q==>>VQC[(((99s###77fa!&&)m_----==AGGII%%%>>""1<<0F1I===??##ALL1VAY>>>??##BOO4q	AAA<<  .&);;;177***!'')))::<====	4B	4B'"DRXXrttR!V,	$BQrxxzz\

lrxxzz$(
*B >>RXXZZ'''??##BMM2a"((**nDDD$$R]]3q288::~EEE$$R__5BBBMM""2==1BrxxzzMAbhhjjL() * )4'''"((***&!DAq+D1Aq!Qd<<)).!##)DL,,((acc	BKaAq|(D"&133	8A <<133>>a A%%%>>\)))==K'''==!!!,,/1339<<<>>""1<<0ACC!##I===<<  .!##)acc/ACC2GGGG<<A&&q)a#a&j1s1v:.E-LBQK!c!f*$P, ., , , ,$&&&%%%r2   c                     t        d      \  } }t        d      }t        d      }t        d|       }t        d      \  }}t	        d|||||||j
                  z        }|j                  |k(  sJ |j                  j                  dk(  sJ |j                  j                  |j                        | |j
                  z  k(  sJ |j                  |j                        t        g d	d
t        |      t        |      gd
t        |       t        |      gg      k(  sJ |j                  j                  |j                        | |z  |j                   z  k(  sJ t        d      }t        d      }t        d      }t        d|       }t        d      \  }}t	        d||||||||j
                  z  |j                   	      }|j"                  |j                  j                   k(  sJ |j                  |j                        t        t        |      t        |      d
gt        |       t        |      d
gg dg      k(  sJ |j                  j                  |j                        d
k(  sJ |j                  j                  |j                        ||z  |j$                  z  k(  sJ t        d      \  }	}
}}}}|	|f|
|f||ffD ci c]  \  }}|j'                  t(              | }}}t        d      t        d      }}t+        d||||      }|j                  j                  |j                        t-        d      k(  sJ |j                  j                  |j                        ||j                  j$                  z  k(  sJ |j                  j                  |j                        d
k(  sJ |j                  j                  |j                        ||j                  j$                  z  k(  sJ t        d      t        d      }}t        d      }t/        d|||	|
|||||j$                  z  |j
                  
      }|j                  j                  |j                        t        t        |	      d
t        |	      gg dt        |	       d
t        |	      gg      k(  sJ |j                  j                  |j                        ||j$                  z  |
|j
                  z  z   k(  sJ |j                  j                  |j                        ||j
                  z  k(  sJ |j                  j                  |j                        j1                  |      | |j
                  z  ||z  |j                   z  z
  k(  sJ t        d      t        d      }}t        d      }t3        d|||	|
|g|||g|||j                   z  	      }|j                  }|j                  j                  |j                        t        g d	d
t        |	      t        |	       gd
t        |	      t        |	      gg      k(  sJ |j                  j                  |j                        | |j                   z  |
|j
                  z  z   ||j                   z  z   k(  sJ |j                  j                  |j                        ||z  |j
                  z  ||j
                  z  z   ||j                   z  z   k(  sJ t        d      t        d      }}t        d      t        d      }}t5        d||||||j$                  z  ||j
                  z        }|j                  |      t-        d      k(  sJ |j                  j                  |j                        ||j$                  z  ||j
                  z  z
  k(  sJ |j                  j                  |j                        d
k(  sJ y c c}}w )Nm lr&   r#   )massrs   r8   ru   rp   	J_P_framer>   r   r   r   )rr   ru   rp   rq   r   r   r>   z	q1:4 u1:4r@   P_frame)rr   ro   rq   r   r>   r   C_frame)rr   ru   ro   rp   )r   r   r   r   r   r,   ru   rr   rv   rz   ry   dcmr	   r   r   r   r}   rq   r+   rW   rX   r   r   r   xreplacer   r   )r   r   r   r#   r&   rC   rA   r8   r   rI   rb   rc   rM   rd   re   qiui	qdot_to_us                     r0   test_particle_compatibilityr      s   5>DAqS!GA1A&!DAqaAq7]	,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&!DAqaAqG")q799}#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//0AEGII4EEEE+K8BBB02Bx"bB8.LMFBRMIMC=(3-qAsAq!Q'A""1#5#56#a&@@@<<  .!a6I6I6K6K2KKKK<<A//0A555<<A//0A8K8K8M8M4MMMMC=(3-qAY'GaBBg&''))m			KA""1#5#56&	R!SWyCG8QB*@BB ;C C C C<<  .!gii-"wyy.2PPPP<<A//0BNBBB<<A../88C	gii!b&799,,. . . C=(3-qAY'GCArB8R"b%,!gii-	IA!!G""1#5#56&As2wR)As2wB+@BB ;C C C C<<  .	
WYYgii'"wyy.8: : :<<A//0	BR'))^+b799n<> > > C=(3-qA%i0.2KWG#q!w#*WYY'))m	-A ;;w3q6)))<<  .!gii-!gii-2OOOO<<A//0A555O Ns     ^?c            
         t        d      \  } }t        d      }t               5  t        d      }t        d| |      }d d d        t	        d      \  }}t        d||||j                  z         |j                  |k(  sJ |j                  j                  dk(  sJ |j                  j                  |j                        | |j                  z  k(  sJ |j                  j                  |j                        t        g d	d
t        |      t        |      gd
t        |       t        |      gg      k(  sJ |j                  j                  |j                        | |z  |j                   z  k(  sJ y # 1 sw Y   9xY w)Nr   r&   r#   r   r%   rs   r8   rp   r   r   r   )r   r   r   r   r   r   r,   r%   rv   rz   ry   r   r	   r   r   r   r}   )r   r   r   r#   r&   rC   rA   s          r0   test_body_compatibilityr      sU   5>DAqS!G		! -I1G,- &!DAqS!Q1!gii-877g77<<9$$$<<  .1"qss(:::77;;qww69,-s1vs1v+>,-AwA+?+A $B B B B <<AGG$Q444- -s   E55E?c            	         t        d      \  } }t        d      \  }}t        d      \  }}t        d      }t        d      }t        d      }t        d|      }	t        d	||
      }
t        d||
      }t	        d|	|
|| | |j
                  z  |	j                  j                        }t	        d|
|||| |j
                  z  |
j                  j                        }|j                  |      t        t        |       t        |        dgt        |       t        |       dgg dg      k(  sJ |j                  |      t        t        |      t        |       dgt        |      t        |      dgg dg      k(  sJ t        |j                  |            t        t        | |z         t        | |z          dgt        | |z         t        | |z         dgg dg      k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      ||j                  z  ||j                  z  z   k(  sJ |j                  t        || j                  t               z
  g      k(  sJ |j                  t        ||j                  t               z
  g      k(  sJ |
j"                  j%                  |      ||z  |j&                  z  k(  sJ |j"                  j%                  |      ||z  |j&                  z  k(  sJ |j"                  j%                  |      ||z  |j&                  z  |||z   z  |j&                  z  z   k(  sJ y )Nzq1 q2zu1 u2r   r!   r"   Br&   r$   r#   r%   r   Rrm   )rU   rw   rp   rq   J2r   r   )r   r   r   r   r   r+   r%   r}   r   r	   r   r   r
   
ang_vel_inrx   rW   rX   rz   r   r,   )rI   rb   rM   rd   r   r   r!   r"   r   r&   PartPPartRrm   r   s                 r0   test_pin_joint_double_pendulumr      s    G$FBG$FB5>DAqsAsAsA#QAc+Ec+E	$5 bf
<B	$uRR bf
@B 558vB#b'15 #BR!4iA B B B B558vB#b'15 #BR!4iA B B B BAEE!H#b2g,R"Wq)I25b2g,BGa1P1:)< "= = = =
 <<?b133h&&&<<?b133h&&&<<?b133hacc1111 77fb2771:o.////77fb2771:o.//// "ad133h..."ad133h..."ad133hBGQSS&@@@@r2   c            
         t        d      \  } }}}}t        d      \  }}}}t        d      }	t        d      }
t        d      }||dz  z
  dz  }|dz  |dz  z   }t        d|
| 	      }t        d
||      }t        d|	      }t	        d||||||
j
                  z  |	j                        }t	        d||||||
j
                  z  |
j
                        }|
j                  |	      t        t        |      dt        |       gg dt        |      dt        |      gg      k(  sJ |
j                  |      t        t        |      t        |       dgt        |      t        |      dgg dg      k(  sJ |j                  |	      t        t        |      t        |      z  t        |      t        |       t        |      z  gt        |       t        |      z  t        |      t        |      t        |      z  gt        |      dt        |      gg      k(  sJ |
j                  |	      ||	j                  z  k(  sJ |
j                  |      | |
j
                  z  k(  sJ |	j                  |      | |	j                  z  ||
j
                  z  z
  k(  sJ |j                  t        ||j                  t              z
  g      k(  sJ |j                  t        ||j                  t              z
  g      k(  sJ |j                  j!                  |j                        ||
j
                  z  k(  sJ |j                  j!                  |j                        | |
j
                  z  k(  sJ |j                  j#                  |	      |dz  |dz  z
  |z  |
j$                  z  k(  sJ |j                  j#                  |	      |dz  |dz  z
  |z  |dz  |dz  z   |z  z   |
j$                  z  k(  sJ y )NzmA, mB, lA, lB, hztheta phi omega alphar!   r"   r   r)      rodr   plater   r&   r$   rm   )rw   rU   rp   rq   r   )rw   rU   ro   rq   r   r   r   )r   r   r   r   r   r}   r,   r   r	   r   r   r   rx   rW   rX   rz   ry   r   r+   )mAmBlAlBhthetaphiomegaalphar!   r"   r   lCr   r   r&   rm   r   s                     r0   test_pin_joint_chaos_pendulumr     s    34BBA-.EFE3usAsAsA
q1u*	B
Q$1*B
E
,CgBa0E#QA	$3E% f
6B	$UE!!##v!##
7B 558vE
AE
{;( #E
As5z:< = = = = 558vC3s8)Q7 #C#c(A6(* + + + + 558v	S#e*	c#hUCH(<=
c(3u:	s3xS#e*)<=	UQE
#% & & & & <<?eACCi'''<<?ufQSSj(((<<?ufQSSj594444 77feejjm34555577fechhqk123333 <<  0BqssF:::>>""5#3#34"qss
BBB >>a QqS2a4Z$6qss$::::"!bd
E'9()!bd
E'9(:;<33'? ? ? ?r2   c                     t        d      \  } }t               \  }}t        d      t        d      ct        t        fd       t        t        fd       j                  ||j                  |        j                  ||j                  |        t        t        fd       t        t        fd       t               \  }}t        d      j                  |t        dz  t        t        d	z  fd
       t        d| ||j                  j                   j                  	       t        |j                  |            t        dt        d	      t!        |       z  dz  t        d	       t#        |       z  dz  gt        d      dz  t        d      dt#        |       z  t!        |       z   z  dz  t        d      t#        |        dt!        |       z  z   z  dz  gt        d      dz  t        d      dt#        |       z  t!        |       z   z  dz  t        d       t#        |       dt!        |       z  z   z  dz  gg      z
  t%        d	      k(  sJ |j'                  |      |j                  z  k(  sJ j(                  j+                  j(                        |j                  |j                  z   k(  sJ j(                  j-                  |      ||j                  z  k(  sJ j(                  j-                        t/        d      k(  sJ j(                  j-                        ||j                  z  k(  sJ t               \  }}t        d      j                  |dt        z  d	z  t         t        dz  fd
       t        d| ||j                   j                  j                  j                  z   	       t        |j                  |            t        t        d       t#        |       z  dz  t        d	       t!        |       dz
  z  dz  t!        |       dz  z
  t1        d      dz  z
  t        d	      t!        |       dz   z  dz  t!        |       dz  z
  t1        d      dz  z   gt!        |       t        d      t        d      z   t#        |        z  dz  t        d       t        d      z   t#        |       z  dz  gt        d      t#        |       z  dz  t        d	      t!        |       dz   z  dz  t!        |       dz  z   t1        d      dz  z
  t        d	      dt!        |       z
  z  dz  t!        |       dz  z   t1        d      dz  z   gg      k(  sJ |j'                  |      t        d      |z  dz  |j                  z  t        d      |z  dz  |j                  z  z   k(  sJ j(                  j+                  j(                        |j                   |j                  z
  k(  sJ j(                  j-                  |      j                         t        d       t        d      z
  |z  dz  |j                  z  t        d       t        d      z   |z  dz  |j                  z  z   k(  sJ j(                  j-                        t/        d      k(  sJ t               \  }}t        d      t        d      cj                  |t         dz  t        t        dz  fd
       j                  |dt        z  d	z  t         t        dz  fd
       t        d| ||j                  |j                  z
  j                   j                  j                  z   
       t        |j                  |            t        t!        |       t        d      t        d      z   t#        |        z  dz  t        d       t        d      z   t#        |       z  dz  gt        d       t#        |       z  dz  t        d	       t!        |       dz   z  dz  t!        |       dz  z
  t1        d      dz  z   t        d	      t!        |       dz
  z  dz  t!        |       dz  z
  t1        d      dz  z
  gt        d      t#        |       z  dz  t        d	      t!        |       dz
  z  dz  t!        |       dz  z   t1        d      dz  z   t        d	       t!        |       dz   z  dz  t!        |       dz  z   t1        d      dz  z
  gg      k(  sJ |j'                  |      t        d      |z  dz  j                  z  t        d      |z  dz  j                  z  z   k(  sJ j(                  j+                  j(                        |j                  |j                  z
  |j                  z   k(  sJ t        d       t        d      z   |z  dz  |j                  z  }j(                  j-                  |      j                         |k(  sJ j(                  j-                        j                         |k(  sJ j(                  j-                        t/        d      k(  sJ y )Nrs   r.   r/   c                  "    t        d       S Nr8   rr   r   r&   r#   r.   s   r0   r:   z+test_pin_joint_interframe.<locals>.<lambda>D      xQTJ r2   c                  "    t        d       S Nr8   ru   r   r&   r/   r#   s   r0   r:   z+test_pin_joint_interframe.<locals>.<lambda>E      xQDI r2   c                  "    t        d       S r   r   r   s   r0   r:   z+test_pin_joint_interframe.<locals>.<lambda>I  r   r2   c                  "    t        d       S r   r   r   s   r0   r:   z+test_pin_joint_interframe.<locals>.<lambda>J  r   r2   r   r@   xyzr8   )ro   rp   rr   rq   g      r)      r   )ro   rp   ru   rq   r>   rt   )r   r1   r   r   rV   r*   r}   orient_body_fixedr   r   r+   r,   r
   r   r	   r   r   r   r   r   rz   ry   r   r   r   )	rC   rA   r!   r"   N_v_Cr&   r/   r#   r.   s	        @@@@r0   test_pin_joint_interframer   ?  s#	   &!DAq!JAq!Q')?JD$
:JK
:IJQQQQ
:JK
:IJ!JAq!Q&!D1rAvr262E:S!Q1133QSSD#8AEE!H	a3q6!A%Qx#a&'81'<=	a1d1gSVc!f!459	aSVGa#a&j(	)A	-	/	a1d1gc!fs1v!56:
q'SVa#a&j(	)A	-	/	(0 !1 1
 5:!H= = = <<?a$&&j(((<<  .!##);;;<<A!acc')))<<D!VAY...<<D!QW,,,!JAq!Q&!D1q2vzB3Q7?S!Q1ACC4QSS"qssQSSy:AEE!H
q'CF	Q	
q'SVaZ	 1	$s1vz	1AaD1H	<	aCFQJ	!	#c!fqj	01Q4!8	;	= 
Q$q'DG#Aw.27(T!W
A	&	*	,	a3q6	A		aCFQJ	!	#c!fqj	01Q4!8	;	aAAJ	!	#c!fqj	01Q4!8	;	=)> "? ? ? ? <<?d1gkAo3d1gkAo6KKKKK<<  .ACC%!##+===<<A'')	a47a. "#.$&'cc.*Qx$q'!Q-')*-+-.SS-1.1 1 1 1 <<D!VAY...!JAq!Q')?JD$1sQwBF3U;1q2vzB3Q7?S!Q11339133$#d) AEE!H	Q$q'DG#Aw.27(T!W
A	&	*	,
q'CF	Q	
q'SVaZ	 1	$s1vz	1AaD1H	<	aCFQJ	!	#c!fqj	01Q4!8	;	= 
a3q6	A		aCFQJ	!	#c!fqj	01Q4!8	;
q'SVaZ	 1	$s1vz	1AaD1H	<	>)? "@ @ @ @ <<?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            
         t        d      \  } t        d      \  }t        d| j                        }|j                  j                  k(  sJ j                  |      t        t              dt              gg dt              dt               gg      k(  sJ t        d      \  }t        d| j                        }|j                  j                  k(  sJ j                  |      t        t               dt              gg dt              dt              gg      k(  sJ t               \  }t        d| j                  j                   j                        }|j                  j                  k(  sJ j                  |      t        t               t               dgt               t              dgg dg      k(  sJ t               \  }t        d| j                  j                   j                        }|j                  j                  k(  sJ j                  |      t        g ddt              t              gdt              t               gg      k(  sJ t        d      \  }t        t        fd	       t        t        fd
       t        t        fd       t        t        fd       t        t        fd       t        d      \  }t        dj                  j                  z          t        t        fd       t        t        fd       y )Nrs   Tr8   rr   ru   rq   r   r   r   )r   r   r   r   r   r   c            	          t        d t              j                  z  t              j                  z  z         S Nr8   )rq   )r   r   r+   r   r,   r&   r!   r#   rC   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s3    xQ36q6ACC<#a&133,3N P r2   c                  6    t        d  j                        S r   r   r+   r&   r#   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s    xQaccB r2   c                  P    t        d j                   j                  z         S r   r   r+   r,   r   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s    xQaccACCiH r2   c            	      T    t        d j                   j                  z         S Nr8   r   r   r&   r/   r#   r.   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s&    xQTD66ACC< ! r2   c            	      T    t        d j                  j                  z         S r   r   r   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s&    xQTD33< ! r2   c            
      8    t        d t        d            S )Nr8   r   r   )r   r   r   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s    hQTD!9 r2   c            	      T    t        d j                  j                  z         S r   )r   r,   r   s   r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>  s&    hQTD33<! r2   )r   r1   r   r,   rq   r   r	   r   r   r}   r+   r   rV   	Exception)	rA   r"   pinr&   r/   r!   r#   r.   rC   s	      @@@@@@r0   test_pin_joint_joint_axisr     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                  /   t        d      \  } }t               \  }}}}t        d|||j                          |j                   j	                  |j                        dk(  sJ |j                  j                  |       |j                  k(  sJ |j                  |      t        g ddt        |        t        |        gdt        |        t        |       gg      k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      j                         t        |dz        k(  sJ |j                  j                  |j                        dk(  sJ |j                  j                  |j                        j                  |      j                         dk(  sJ |j                  j!                  |      dk(  sJ t               \  }}}}t        d|||j"                  |j                         |j"                  j	                  |j                        dk(  sJ |j"                  j                  |      |j                  k(  sJ |j                  |      t        dt        |        t        |        gg ddt        |        t        |       gg      k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      j                  |      ||j"                  z  k(  sJ |j                  |      j                         t        |dz        k(  sJ |j                  |      j%                  |j"                        dk(  sJ |j                  j!                  |      ||j&                  z  k(  sJ |j                  j                  |j                        |j                   k(  sJ |j                  j                  |j                        j                  |      j                         t        |       |j"                  z  t        |       |j&                  z  z   k(  sJ |j                  j!                  |      j	                  |j                        t(        dz  k(  sJ t               \  }}}}t        d|||j"                  |j                  	       |j"                  j	                  |j                        dk(  sJ |j"                  j                  |      |j                  k(  sJ |j                  |      t        g d
t        |        dt        |       gt        |       dt        |       gg      k(  sJ |j                  |      ||j"                  z  k(  sJ |j                  |      j                  |      ||j                  z  k(  sJ |j                  |      j                         t        |dz        k(  sJ |j                  |      j	                  |j                        }|j+                  |di      dk(  sJ |j                  j!                  |      dk(  sJ |j                  j                  |j                        |j                  k(  sJ t               \  }}}}t        d|||j                  |j                  |j                  |j"                  z          t-        |j                  j	                  |j                  |j"                  z               dk(  sJ |j                  |j"                  z   j                  |      j                         t        d      |j                  z  k(  sJ t        |j                  |            t        t        d      dz  t        d       t        |       z  dz  t        d       t        |       z  dz  gt        d      dz  t        d      t        |       z  dz  t        d      t        |       z  dz  gdt        |        t        |       gg      k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      j                  |      j                         ||j                  z  ||j"                  z  z   t        d      z  k(  sJ |j                  |      j                         t        |dz        k(  sJ |j                  |      j	                  |j                  |j"                  z         }|j+                  |di      dk(  sJ |j                  j!                  |      j                         ||j&                  z  t        d      z  k(  sJ |j                  j                  |j                        |j                  |j                  z
  k(  sJ |j                  j                  |j                        j                  |      j                         dt        d      dz  z
  |j                  z  t        d      t        |       z  dz  |j"                  z  z   t        d      t        |       z  dz  |j&                  z  z   k(  sJ |j                  j!                  |      j                  |      j                         t        d       |z  t        |       z  dz  |j"                  z  t        d      |z  t        |       z  dz  |j&                  z  z   k(  sJ |j                  j!                  |      j	                  |j                        t(        dz  k(  sJ t               \  }}}}t        d|||j                  |j                  |j                  |j"                  z   |j&                  z
         t-        |j                  j	                  |j                  |j"                  z   |j&                  z
              dk(  sJ |j                  |j"                  z   |j&                  z
  j                  |      j                         t        d      |j                  z  k(  sJ t        |j                  |            t        t        d      dz  t        d       t        | t(        dz  z         z  dz  t        d      t        | t(        dz  z         z  dz  gt        d      dz  t        d      t        | t(        dz  z         z  dz  t        d      t        | t(        dz  z         z  dz  gt        d       dz  t        d      t        | dt(        z  dz  z         z  dz  t        d      t        | dt(        z  dz  z         z  dz  gg      k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      j                  |      j                         ||j                  z  ||j"                  z  z   ||j&                  z  z
  t        d      z  k(  sJ |j                  |      j                         t        |dz        k(  sJ |j                  |      j	                  |j                  |j"                  z   |j&                  z
        }|j+                  |di      j                         dk(  sJ |j                  j!                  |      j                         ||j"                  z  ||j&                  z  z   t        d      z  k(  sJ |j                  j                  |j                        |j                  |j                  z
  k(  sJ |j                  j                  |j                        j                  |      j                         dt        d      dz  z
  |j                  z  t        d      t        | t(        dz  z         z  dz  |j"                  z  z   t        d      t        | t(        dz  z         z  dz  |j&                  z  z
  k(  sJ |j                  j!                  |      j                  |      j                         t        d      |z  t        | t(        dz  z         z  dz  |j"                  z  t        d      |z  t        | t(        dz  z         z  dz  |j&                  z  z   k(  sJ |j                  j!                  |      j	                  |j                        t(        dz  k(  sJ t               \  }}}}t/        d      \  }}t        d||||j                  z  ||j                  z  |j                  |j"                  z   |j&                  z
  |j                  |j"                  z
  |j&                  z          |j                  |j"                  z
  |j&                  z   j	                  |j                  |j"                  z   |j&                  z
        }t-        |      dk(  sJ |j                  |j"                  z
  |j&                  z   j                  |      j                         dt        |       z  dz  t1        d      dz  z
  |j                  z  t1        d      dz  dt        | t(        dz  z         z  dz  z
  |j"                  z  z   dt        | t(        dz  z         z  dz  t1        d      dz  z
  |j&                  z  z   k(  sJ t        |j                  |            t        t1        d      dz  dt        |       z  dz  z
  dt        | t(        dz  z         z  dz  t1        d      dz  z
  dt        | t(        dz  z         z  dz  t1        d      dz  z   gdt        | t(        dz  z         z  dz  t1        d      dz  z   dt        |       z  dz  t1        d      dz  z
  dt        | t(        dz  z         z  dz  t1        d      dz  z   gdt        | t(        dz  z         z  dz  t1        d      dz  z
  dt        | t(        dz  z         z  dz  t1        d      dz  z   dt        |       z  dz  t1        d      dz  z
  gg      k(  sJ |j                  |      ||j                  z  ||j"                  z  z
  ||j&                  z  z   t        d      z  z
  j                         sJ |j                  |      j                  |      j                         ||j                  z  ||j"                  z  z   ||j&                  z  z
  t        d      z  k(  sJ |j                  |      j                         t        |dz        k(  sJ |j                  |      j	                  |j                  |j"                  z   |j&                  z
        }|j+                  |di      j                         dk(  sJ |j                  j!                  |      j                         t        d      |z  |z  dz  |j"                  z  t        d      |z  |z  dz  |j&                  z  z   k(  sJ |j                  j                  |j                        ||j                  z  ||j                  z  z
  k(  sJ |j                  j                  |j                        j                  |      j                         ||dt        |       z  dz
  z  dz  z   |j                  z  |dt        | t(        dz  z         z  dz   z  dz  |j"                  z  z   |dt        | t(        dz  z         z  dz   z  dz  |j&                  z  z
  k(  sJ |j                  j!                  |      j                  |      j                         d|z  |z  t        |       z  dz  |j                  z  d|z  |z  t        | t(        dz  z         z  dz  |j"                  z  z   d|z  |z  t        | t(        dz  z         z  dz  |j&                  z  z   k(  sJ |j                  j!                  |      j3                  |j                  |j"                  z
  |j&                  z         j                         dk(  sJ y )Nrj   r8   r   r   r   r)   r   r   rr   ro   r   r>   ro   rp   ru   r@   r   r         m nro   rp   ru   rr   r   )r   r1   r   r+   angle_betweenr   r   r	   r   r   r   	magnituder   rz   ry   r
   r   r,   crossr}   r   r   r   r   r   dot)	rC   rA   r!   r"   r#   r&   angler   ns	            r0   test_pin_joint_arbitrary_axisr     s   *%DAq  !JAq!QS!Q!##.SSD$)))CCKKN?acc!!!558vz !CF7SVG4 !CF7CF35 6 6 6 6 <<?ae###<<?$$&$q!t*444<<  .!333<<  .66q9BBDIII<<A!###  !JAq!QS!Q!##>33QSS!Q&&&33;;q>QSS   558vCF7SVG4( !CF7CF35 6 6 6 6 <<?ae###<<?""1%QSS000<<?$$&$q!t*444<<?  %***<<A!ACC%'''<<  .133$666LL!!!,,/77:CCEF133JQ#$ % $<<A,,QSS1RT999  !JAq!QS!Q!##ACC@33QSS!Q&&&33;;q>QSS   558vy!$QCF3 #A3q624 5 5 5 5 <<?ae###<<?""1%133...<<?$$&$q!t*444LLO))!##.E>>1a&!Q&&&<<A!###<<  .!##555  !JAq!QS!QQSSaccccACCi)acc''acc	23q888CC!##Iq!**,Q;;;AEE!H	aT!WHSVOA%QxAq'89	aDGCFN1$d1gc!fnQ&67	
SVGSV) "    <<?ae###LLO##A&//1qssUQqssU]DG#$ % $<<?$$&$q!t*444LLO))!##)4E>>1a&!Q&&&<<A'')a!##gtAw->>>><<  .!##);;;LL!!!,,/77:CCEa]ACC$q'#a&."2133"66GCFN1QSS !! " ! LLQ''*335!WHQJs1va#d1gaiA&6q&8&<<= > =<<A,,QSS1RT999!JAq!QS!QQSSaccccACCi!##o/acc''acc	ACC89Q>>>CC!##IO$$Q'002d1gacckAAAAEE!H	aT!WHSRT]*1,	aQAX	q	 	"	aDGCBrE	N*1,	aQBY		!	#
q'!T!WSQrT"W--a/	aQ2b[!	!!	#	%)& "' ' ' ' <<?ae###<<?""1%..0QqssUQqssU]56qssU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!JAq!Q5>DAqS!QQW!acc'ccACCi!##o ssQSSy1330 SS133Y_++ACC!##IO<Ee!!!SSWQSS[!!!$--/AYq[1Q46!133&!A$q&1SRT]?13D*Dacc)IIs1r!t8}_Q1a',-- . - AEE!H	
1a!CF(1*	bQAX.q01Q469	
3q2a4x=	QqT!V	#	%	
3q2a4x=	QqT!V	#Qs1vXaZ!A$q&%8	
3q2a4x=	QqT!V	#	%	CBqDM	!	ad1f	$aA1Hoa&7!A$q&&@	
3q6!ad1f		) "        LLOququ}qu4d1g==GGIII<<?""1%..0QqssUQqssU]56qssU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<<  .!ACC%!ACC%-???LL!!!,,/77:CCEAc!fHqL!!##QSS(1aA1Ho/ ,,, !CBqDM/A-.q0455 6 5 LLQ''*335E!GCFN1QSS 1Q3q5QAX#6q#8#<<aCE#a"Q$h-!!##%&& ' & <<A""1339qss?3<<>!CCCr2   c                  <   t               \  } }}}t        j                  ||j                   |j                        }|j                  |j                  k(  sJ t        j                  ||j
                   |j
                        }|j                  |j                  k(  sJ t        j                  ||j                   |j                        }|j
                  |j
                  k(  sJ t        j                  ||j                   |j
                  z
  |j                  |j
                  z         }|j                  |j                  k(  sJ t        j                  ||j
                   |j                  z
  |j
                  |j                  z         }|j                  |j                  k(  sJ t        j                  ||j                   |j                  z
  |j                  |j                  z         }|j
                  |j
                  k(  sJ t        j                  ||j                   |j
                  z
  |j                  z
  |j                  |j
                  z   |j                  z         }|j
                  |j                  z
  |j
                  |j                  z
  k(  sJ y N)r1   r   _create_aligned_interframer+   r}   r,   )r!   r"   r#   r&   fs        r0   test_create_aligned_frame_pir   R  s   !JAq!Q((QSSD!##6A33!##::((QSSD!##6A33!##::((QSSD!##6A33!##::((QSSD133Jacc	BA33!##::((QSSD133Jacc	BA33!##::((QSSD133Jacc	BA33!##::((QSSD133J,<accACCi!##oNA339acc	!!!r2   c            
        	 t        d      \  	} t        d      \  }}}t        d	| ||      }|j                  |j                  k(  sJ t        dt        	      t        	      gdt        	       t        	      gg dg      }t        d      \  }}}t        d	| ||j                         j                  |      |k(  sJ t        d      \  }}}t        d	| |||j                          j                  |      |k(  sJ t        d      \  }}}t        t        	fd       y )	Nzq uTr8   )rr   ru   r   r   r   c                  <    t        d j                  z        S r   )r   r}   r   s   r0   r:   z%test_pin_joint_axis.<locals>.<lambda>x  s    xQa!##gF r2   )r   r1   r   rq   r+   r	   r   r   r}   r   r   rV   )
rA   r"   r.   r/   r8   N_R_Ar&   r!   r#   rC   s
         @@@@r0   test_pin_joint_axisr   d  sE   % DAq+D1Aq!QdaAqD4PA<<466!!!QAA'QQ(  E ,D1Aq!QdS!Q1t558u+D1Aq!QdS!Q1t!558u+D1Aq!Qd
:FGr2   c                     t               \  } t        dj                  j                  z         }|j                  j
                  dk(  sJ |j                  j                  j                        j                  j                  z   k(  sJ |j                  j                  k(  sJ t               \  } j                  j                  dj                  j                  z         }t        d|j                        }|j                  |k(  sJ |j                  j                  k(  sJ t               \  } t        t        fd       t        d      t               \  } t        t        fd       t               \  } j                  j                  d| j                  z        t        t        fd	       t        d      t        t        fd
       y )Nr8   ro   	J_P_jointp)ro   rp   c                  T    t        d j                  j                              S Nr8   r   )r   r+   	to_matrix)r&   r!   r#   s   r0   r:   z'test_locate_joint_pos.<locals>.<lambda>  s    8CAACCMM!4DE r2   rC   c                  <    t        d j                  z        S r   r   r   s   r0   r:   z'test_locate_joint_pos.<locals>.<lambda>  s    xQACCH r2   c                  "    t        d       S Nr8   r   r   r&   r#   rp   s   r0   r:   z'test_locate_joint_pos.<locals>.<lambda>      xQ{K r2   c                  "    t        d       S r   r   r   s   r0   r:   z'test_locate_joint_pos.<locals>.<lambda>  r   r2   )r1   r   r,   r}   ro   rv   ry   rz   rp   r   r   r;   r   rV   r   )r"   jointro   r&   r!   r#   rp   rC   s      @@@@@r0   test_locate_joint_posr  {  s   !JAq!QS!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                  b   t               \  } }t        d      j                  | | j                  d       t	        d      }|j
                  k(  sJ |j
                  j                  |       dk(  sJ |j                  |k(  sJ t        d      }t               \  } }t        d      j                  | | j                  |       t        t        fd       t               \  } }t        d      j                  | | j                  d       t        t        fd       y )	N	int_framer>   r8   r   r   rC   c                  "    t        d       S r   r   )r&   r#   rr   s   r0   r:   z)test_locate_joint_frame.<locals>.<lambda>  s    8CA9JK r2   c                  "    t        d       S r   r   )r&   r#   ru   s   r0   r:   z)test_locate_joint_frame.<locals>.<lambda>  s    8CA8HI r2   )r1   r   r*   r}   r   rr   r   ru   r   r   rV   )r!   r"   r  rC   r&   r#   ru   rr   s       @@@@r0   test_locate_joint_framer    s   !JAq!Q&{3!!!QSS!,S!Q2CDE""&7777""--a0A555!!Q&&&sA!JAq!Q&{3!!!QSS!,
:KM  !JAq!Q%k2  ACC+
:IKr2   c            	      D   t               \  } } }}t        d      \  }}t        d||      }|j                  dk(  sJ |j                  |k(  sJ |j
                  |k(  sJ |j                  t        |g      k(  sJ |j                  t        |g      k(  sJ |j                  t        ||j                  t              z
  g      k(  sJ |j                  |j                  j                  k(  sJ |j                  j!                  |j"                        t%        d      k(  sJ |j&                  j!                  |j"                        t%        d      k(  sJ |j&                  j!                  |j                        | |j                  j                  z  k(  sJ |j"                  j!                  |j"                        | |j                  j                  z  k(  sJ |j"                  j)                  |j                        ||j                  j                  z  k(  sJ |j                  j+                  |j                        dk(  sJ |j                  j+                  |j                        dk(  sJ  |j,                         dk(  sJ t               \  }}}}t/        d      \  }}	t1        d      }
|
j3                  |j                  |j4                  t6        dz         t        d||||j                  j                  z  |	|j                  j4                  z  |j                  j8                  |
      }|j                  |j                  j8                  k(  sJ |j                  j!                  |j"                        |	|j                  j4                  z  k(  sJ |j&                  j!                  |j"                        ||j                  j                  z  k(  sJ |j&                  j!                  |j                        | |j                  j8                  z  k(  sJ |j"                  j!                  |j"                        | |j                  z  ||j8                  z  z
  |	|j4                  z  z   k(  sJ |j"                  j)                  |j                        ||j                  j8                  z  k(  sJ |j"                  j)                  |
      t%        d      k(  sJ |j                  j+                  |j                        dk(  sJ |j                  j+                  |j                        dk(  sJ t               \  } } }}t1        d      }
|
j3                  |j                  |j4                  t6        dz         t        d||||j                  j8                  z  |	|j                  j                  z  |j                  j8                  |
      }|j                  |j                  j8                  k(  sJ |j                  j!                  |j"                        |	|j                  j                  z  k(  sJ |j&                  j!                  |j"                        ||j                  j8                  z  k(  sJ |j&                  j!                  |j                        | |j                  j8                  z  k(  sJ |j"                  j!                  |j"                        | |z
  |j                  j8                  z  |	|j                  j                  z  z   k(  sJ |j"                  j)                  |j                        ||j                  j8                  z  k(  sJ |j                  j+                  |j                        dk(  sJ |j                  j+                  |j                        dk(  sJ y )	Nq_S, u_Sr   r   z&PrismaticJoint: S  parent: P  child: Cri   r'   r)   rn   )r1   r   r   rv   r4   r5   rw   r	   rU   rx   rW   rX   rq   r%   r+   rp   ry   rz   r   ro   r   r   r|   r   r   r*   r,   r   r}   )rf   r#   r&   rC   rA   r   r!   r"   r   r   r.   s              r0   test_prismatic_jointr    s   !JAq!Q*%DAqsAq!A66S==88q==77a<<==FA3K'''88vqc{"""66VQ]O,,,,<<17799$$$==!!!,,/6!9<<<>>""1<<0F1I===>>""1==1qS17799_DDD<<  .A#		/AAA<<AGG$AGGII55577agg&!+++77agg&!+++199;BBBB!JAq!Q5>DAq'"DQWWacc26*sAqq17799}#$qwwyy=QWWYY)-	/A <<17799$$$==!!!,,/1qwwyy=@@@>>""1<<0A		MAAA>>""1==1qS17799_DDD<<  .A#)a!##g2EACC2OOOO<<AGG$AGGII555<<D!VAY...77agg&!+++77agg&!+++!JAq!Q'"DQWWacc26*sAqq17799}#$qwwyy=QWWYY)-	/A <<17799$$$==!!!,,/1qwwyy=@@@>>""1<<0A		MAAA>>""1==1qS17799_DDD<<  .A26177992Dq{2RRRR<<AGG$AGGII55577agg&!+++77agg&!+++r2   c                      t        d      \  } }t               \  }}}}t        d|||j                          |j                   j	                  |j                        dk(  sJ |j                  j                  |       |j                  k(  sJ |j                  |      t        g dg dg dg      k(  sJ |j                  j                  |j                        | |j                  z  k(  sJ |j                  j                  |j                        j                  |      j                         |  |j                  z  k(  sJ |j                  j                  |      ||j                  z  k(  sJ |j                  j                  |      j                  |      | |j                  z  k(  sJ |j                  |      dk(  sJ |j                  |      dk(  sJ t               \  }}}}t        d|||j                  |j                         |j                  j	                  |j                        dk(  sJ |j                  j                  |      |j                  k(  sJ |j                  |      t        g dg d	g dg      k(  sJ |j                  j                  |      ||j                  z  k(  sJ |j                  j                  |      j                  |      ||j                  z  k(  sJ |j                  j                  |j                        | |j                  z  |j                  z
  k(  sJ |j                  j                  |j                        j                  |      j                         | |j                  z  |j                  z   k(  sJ |j                  |      dk(  sJ |j                  |      dk(  sJ t               \  }}}}t        d|||j                  |j                  
       |j                  j	                  |j                        dk(  sJ |j                  j                  |      |j                  k(  sJ |j                  |      t        g dg dg dg      k(  sJ |j                  j                  |      ||j                  z  k(  sJ |j                  j                  |      j                  |      ||j                  z  k(  sJ |j                  j                  |j                        |j                  | |j                  z  z   k(  sJ |j                  |      dk(  sJ |j                  |      dk(  sJ t               \  }}}}t        d|||j                  |j                  |j                  |j                  z          |j                  j	                  |j                  |j                  z         dk(  sJ |j                  |j                  z   j                  |      t        d      |j                  z  k(  sJ |j                  |      t        t        d      dz  t        d       dz  dgt        d      dz  t        d      dz  dgg dg      k(  sJ |j                  j                  |j                        | dz   |j                  z  |j                  z
  k(  sJ |j                  j                  |j                        j                  |      | t        d      dz  z
  dz   |j                  z  t        d      dz  |j                  z  z   k(  sJ |j                  j                  |      j                  |      ||j                  |j                  z   z  t        d      z  k(  sJ |j                  j                  |      ||j                  z  k(  sJ |j                  |      dk(  sJ |j                  |      dk(  sJ t               \  }}}}t        d|||j                  |j                  |j                  |j                  z   |j                  z
         |j                  j	                  |j                  |j                  z   |j                  z
        j                         dk(  sJ |j                  |j                  z   |j                  z
  j                  |      t        d      |j                  z  z
  j                         dk(  sJ t        |j                  |            t        t        d      dz  t        d       dz  t        d      dz  gt        d      dz  t        d      dz  t!        d      dz  z   t!        d      dz  t        d      dz  z
  gt        d       dz  t!        d      dz  t        d      dz  z
  t        d      dz  t!        d      dz  z   gg      k(  sJ |j                  j                  |j                        | dz   |j                  z  |j                  z
  k(  sJ |j                  j                  |j                        j                  |      | t        d      dz  z
  dz   |j                  z  t        d      dz  |j                  z  z   t        d      dz  |j                  z  z
  z
  j                         dk(  sJ |j                  j                  |      ||j                  z  k(  sJ |j                  j                  |      j                  |      t        d      |z  dz  |j                  z  t        d      |z  dz  |j                  z  z   t        d      |z  dz  |j                  z  z
  z
  j                         sJ |j                  |      dk(  sJ |j                  |      dk(  sJ t               \  }}}}t#        d      \  }}t        d||||j                  z  ||j                  z  |j                  |j                  z   |j                  z
  |j                  |j                  z
  |j                  z          |j                  |j                  z
  |j                  z   j	                  |j                  |j                  z   |j                  z
        j                         dk(  sJ |j                  |j                  z   |j                  z
  j                  |      |j                  |j                  z
  |j                  z   z
  j                         dk(  sJ t        |j                  |            t        t!        d       dz  t!        d       dz  t!        d      dz  gt!        d      dz  t!        d      dz  t!        d      dz  gt!        d       dz  t!        d      dz  t!        d      dz  gg      k(  sJ |j                  j                  |j                        |t        d      | z  dz  z   |j                  z  t        d      | z  dz  |j                  z  z
  t        d      | z  dz  |j                  z  z   ||j                  z  z
  z
  j                  |      j                         dk(  sJ |j                  j                  |j                        j                  |      ||dz  z   t        d      | z  dz  z   |j                  z  d|z  dz  t        d      | z  dz  z
  |j                  z  z   d|z  dz  t        d      | z  dz  z   |j                  z  z   z
  j                         dk(  sJ |j                  j                  |      j                  |      t        d      |z  dz  |j                  z  t        d      |z  dz  |j                  z  z
  t        d      |z  dz  |j                  z  z   z
  j                         dk(  sJ |j                  j                  |      j                  |      t        d      |z  dz  |j                  z  t        d      |z  dz  |j                  z  z   t        d      |z  dz  |j                  z  z
  z
  j                         dk(  sJ |j                  |      dk(  sJ |j                  |      dk(  sJ y )Nr
  r   r   r   r   r   r   r   r   r   r   r   r)   r>   r@   r   r   r   r   )r   r1   r   r+   r   r   r   r	   rz   ry   r
   r   r   r,   r   r}   r   r   )rC   rA   r!   r"   r#   r&   r   r   s           r0   #test_prismatic_joint_arbitrary_axisr    s   *%DAq!JAq!Q31t4SSD$)))CCKKN?acc!!!558vz:yABBBB<<  .!acc'999<<  .66q9BBDQSSPPP<<A!acc')))<<A&&q)aR!##X555<<?a<<?a  !JAq!Q31qssD33QSS!Q&&&33;;q>QSS   558vz9i@AAAA<<A!acc')))<<A&&q)QW444<<  .!ACC%!##+===<<  .66q9BBD!##PQPSPSSSS<<?a<<?a  !JAq!Q31!##F33QSS!Q&&&33;;q>acc!!!558vy*i@AAAA<<A!acc')))<<A&&q)QW444<<  .!##!##+===<<?a<<?a  !JAq!Q31133ACC$%CC!##I/33QSS133Y'1,,,CC!##Iq!T!WQSS[000558vQ	DG8A:q9DGAItAwqyRS;TV_`aaaa<<  .1q5!##+2CCCC<<  .66q9a$q'!)ma>OQRQTQT=TW[\]W^_`W`abadadWd=dddd<<A&&q)Q!##)_T!W-DDDD<<A!ACC%'''<<?a<<?a!JAq!Q31133ACC$%CC!##IO533QSS133Y_-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!ACC%'''LLQ''*Q	!ACC$q'!)A+acc/)DGAIaKO;=>FhjI I I<<?a<<?a!JAq!Q5>DAq311QSS5a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/AAaccEIKgajq) ) ) LL!!!,,/77:	
QqS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                  
   t               \  } }}}t        d      \  }}}}t        d||      }|j                  dk(  sJ |j                  |k(  sJ |j
                  |k(  sJ |j                  t        ||g      k(  sJ |j                  t        ||g      k(  sJ |j                  |k(  sJ |j                  |k(  sJ |j                  |k(  sJ |j                  |k(  sJ |j                  t        ||j                  t              z
  ||j                  t              z
  g      k(  sJ |j                   | j"                  k(  sJ |j$                  j'                  |j(                        t+        d      k(  sJ |j,                  j'                  |j(                        t+        d      k(  sJ |j,                  j'                  |j.                        | | j"                  z  k(  sJ |j(                  j'                  |j(                        || j"                  z  k(  sJ |j$                  j1                  |       || j"                  z  k(  sJ |j3                  |       || j"                  z  k(  sJ |j4                  | k(  sJ |j6                  |k(  sJ |j9                         dk(  sJ t        d      \  }	}
}}t;        d      \  }}t        d      \  } }}}}}t        d|||	|||| j"                  z  ||j<                  z  ||d| j>                  z  	      }|j                  t        |	|g      k(  sJ |j                  t        ||g      k(  sJ |j                  |	k(  sJ |j                  |k(  sJ |j                  |k(  sJ |j                  |k(  sJ |j                  t        ||	j                  t              z
  ||j                  t              z
  g      k(  sJ |j                   d| j>                  z  k(  sJ |j$                  j'                  |j(                        ||j<                  z  k(  sJ |j,                  j'                  |j(                        || j"                  z  k(  sJ |j,                  j'                  |j.                        | | j>                  z  k(  sJ |j(                  j'                  |j(                        || j"                  z  || j>                  z  z   ||j<                  z  z
  k(  sJ |j(                  j1                  |       || j>                  z  ||z  |j>                  z  z
  k(  sJ |j3                  |       || j>                  z  k(  sJ y )
Nzq0:2_J, u0:2_Jr8   r   z(CylindricalJoint: J  parent: P  child: Cz
q0:2, u0:2l, mTr)   )rotation_coordinaterotation_speedtranslation_speedro   rp   rr   ru   rq   ) r1   r   r   rv   r4   r5   rw   r	   rU   r  translation_coordinater  r  rx   rW   rX   rq   r+   rp   ry   rz   r   ro   r{   r   r   rr   ru   r|   r   r,   r}   )r!   r"   r#   r&   q0_defq1_defu0_defu1_defCjrH   rI   rL   rM   r   r   r.   r/   s                    r0   test_cylindrical_jointr  K  s   !JAq!Q%34D%E"FFFF	#q!	$B77c>>99>>88q==>>VVV$4555599/0000!!V+++$$...&&&6)))77ffv{{1~5vA7NOPPPP==ACC>>""1<<0F1I===??##ALL1VAY>>>??##BOO4!##EEE<<  .&133,>>>>>a FQSSL000<<?fqssl***1$$$!###::<EEEE#L1NBB6?DAq+D1Aq!Qd	#q!B,.QW&'!##g+/AG
EB >>VRL111199Bx((((!!R'''$$..."""2%%%77fb2771:orFKKN/BCDDDD==AG###>>""1<<0AG;;;??##ALL1QW<<<??##BOO4!##EEE<<  	QSS6ACC</!acc'9: : :<<A"qss(R!Vacc\"9999<<?b133h&&&r2   c                  ~   t               \  } }}}t        d      \  }}}t        d      \  }}}	t        d||      }
|
j                  dk(  sJ |
j                  |k(  sJ |
j
                  |k(  sJ |
j                  t        |||g      k(  sJ |
j                  t        |||	g      k(  sJ |
j                  |k(  sJ |
j                  t        ||g      k(  sJ |
j                  |k(  sJ |
j                  t        ||	g      k(  sJ |
j                  t        ||j                  t              z
  ||j                  t              z
  |	|j                  t              z
  g      k(  sJ |
j                   | j"                  k(  sJ |
j$                  | j&                  | j(                  gk(  sJ |
j*                  j-                  |j.                        t1        d      k(  sJ |
j2                  j-                  |j.                        t1        d      k(  sJ || j&                  z  || j(                  z  z   }|
j2                  j-                  |
j*                        | k(  sJ |j.                  j-                  |j.                        |k(  sJ |
j*                  j5                  |       || j&                  z  |	| j(                  z  z   k(  sJ |j7                  |       || j"                  z  k(  sJ |
j8                  | k(  sJ |
j:                  |k(  sJ |
j=                         dk(  sJ t        d      \  }}}}}}t?        d      \  }}t        d      \  } }}}}}t        d|||||g||g|| j"                  z  ||j&                  z  ||	
      }
|
j                  t        |||g      k(  sJ |
j                  t        |||g      k(  sJ |
j                  |k(  sJ |
j                  t        ||g      k(  sJ |
j                  |k(  sJ |
j                  t        ||g      k(  sJ |
j                  t        ||j                  t              z
  ||j                  t              z
  ||j                  t              z
  g      k(  sJ |
j                   |j"                  k(  sJ |
j$                  |j&                  |j(                  gk(  sJ |
j*                  j-                  |j.                        ||j&                  z  k(  sJ |
j2                  j-                  |j.                        || j"                  z  k(  sJ |
j2                  j-                  |
j*                        || j&                  z  || j(                  z  z   k(  sJ |j.                  j-                  |j.                        || j"                  z  || j&                  z  z
  || j(                  z  z
  ||j&                  z  z
  k(  sJ |j.                  j5                  |       | | j&                  z  || j(                  z  z
  ||z  |j"                  z  z   k(  sJ |j7                  |       || j"                  z  k(  sJ y )
Nzq0:3_Jzu0:3_Jr8   r   z#PlanarJoint: J  parent: P  child: C
q0:3, u0:3r  T)r  planar_coordinatesplanar_speedsro   rp   rr   ru   ) r1   r   r   rv   r4   r5   rw   r	   rU   r  r  r  r  rx   rW   rX   rotation_axisr+   planar_vectorsr,   r}   rp   ry   rz   r   ro   r   r   rr   ru   r|   r   )r!   r"   r#   r&   r  r  q2_defr  r  u2_defr  r_P_CrH   rI   rb   rL   rM   rd   r   r   r.   r/   s                         r0   test_planar_jointr#  |  s   !JAq!Q+H5FFF+H5FFF	S!Q	B77c>>99>>88q==>>VVVV$<====9978888!!V+++  FFF+;$<<<<&&&vvv&6777777ffv{{1~5vA7N$v{{1~57 8 8 8 8qss"""acc
***>>""1<<0F1I===??##ALL1VAY>>>QSSL6ACC<'E??##BNN3v===<<  .%777>>a FQSSL6ACC<$????<<?fqssl***1$$$!###::<@@@@+L9BBB6?DAq+D1Aq!Qd	S!QB)+RR"#acc'q133w'+d
DB >>VRRL111199B/0000!!R'''  FB8$4444&&&vr2h////77ffrwwqz12
? 2771:o/ 0 0 0 0tvv%%% 0000>>""1<<0AG;;;??##ALL1QW<<<??##BNN3rACCx"qss(7JJJJ<<  	QSS28+b133h6QSS@A A A<<A2#)b133h"6!acc9I"IIII<<?fqssl***r2   c                  >   t        d      \  } }}}}}t        d      \  }}t               \  }}	}
}t        d|
|| ||g|||g||j                  z  | |j                  z  |j                  |j
                  t        d      z  z   |	j                  |	j
                  t        d      z  z         }|j                  j                  |      |j                  |j
                  t        d      z  z   j                         k(  sJ |j                  j                  |	      |	j                  |	j
                  t        d      z  z   j                         k(  sJ |j                  j                  |j                        t        dz  k(  sJ |j                  |	      j                  | d|d|di      t        d      k(  sJ t        t!        |       t        d       t#        |       z  dz  t#        |       dz  gt        d      t#        |       z  dz  dt!        |       z  d	z  d
z   t        d      dt!        |       z
  z  d	z  gt#        |        dz  t        d      dt!        |       z
  z  d	z  t!        |       d	z  dz   gg      }t%        |j                  |	      |z
        t'        d      k(  sJ y )Nr  zl1:3r8   r@   )ro   rp   rr   ru   r   r   r)   r   g      ?r>   g      ?)r   r   r1   r   r}   r,   r   r  r   	normalizer   r   r   r   r   r	   r   r   r
   r   )rH   rI   rb   rL   rM   rd   l1l2r!   r"   r#   r&   r8   r   s                 r0   test_planar_joint_advancedr(    sM    ,L9BBBV_FB!JAq!QCArB8R"b!#acc!#acc	&'ccACC$q'M&9%&SS133a=%8		:A
 ??""1%!##d1g*=)H)H)JJJJ??""1%!##d1g*=)H)H)JJJJ??((-a777558b!RB23s1v===	R47(SW$q(#b'A+6	a3r7	Q	CGa% 7	aABK	 1	$	&
b'AtAw!c"g+.2CGaK%4GH	J KE AEE!Hu$%q111r2   c            	         t               \  } }}}t        d      \  }}}}}}	t        d||      }
|
j                  dk(  sJ |
j                  |k(  sJ |
j
                  |k(  sJ |
j                  t        |||g      k(  sJ |
j                  t        |||	g      k(  sJ |
j                  t        ||j                  t              z
  ||j                  t              z
  |	|j                  t              z
  g      k(  sJ |
j                  j                  |j                        t        d      k(  sJ |
j                   j                  |j                        t        d      k(  sJ |
j                   j                  |
j                        t        d      k(  sJ |j                  j                  |j                        t        d      k(  sJ |j                  j#                  |       t        d      k(  sJ | j%                  |      | t'        |      z  t'        |      z  |t)        |      z  z
  |j*                  z  |t)        |      z  t'        |      z  |t'        |      z  z
  |j,                  z  z   | t)        |      z  |	z
  |j.                  z  z   k(  sJ |j%                  |       |t'        |      z  t'        |      z  |t)        |      z  z   |j*                  z  | t)        |      z  t'        |      z  |t'        |      z  z   |j,                  z  z   |t)        |      z  |	z   |j.                  z  z   k(  sJ  |
j0                         dk(  sJ |
j2                  dk(  sJ |
j4                  dk(  sJ |
j6                  J y )Nq0:3_S, u0:3_Sr   r   z&SphericalJoint: S  parent: P  child: CBODY{   )r1   r   r   rv   r4   r5   rw   r	   rU   rx   rW   rX   rp   ry   rz   r   ro   r   r   r   r   r+   r,   r}   r|   	_rot_type
_rot_order_amounts)r!   r"   r#   r&   rH   rI   rb   rL   rM   rd   r   s              r0   test_spherical_jointr0    s   !JAq!Q+,<=BBBsAq!A66S==88q==77a<<==FBB<000088vr2rl++++66VR"''!*_b2771:orBGGAJOPPPP==!!!,,/6!9<<<>>""1<<0F1I===>>""1==1VAY>>><<  .&);;;<<A&)+++<<?sSW}s2w6c"gELRL3r7"R#b'\1QSSP9 93R=2%<-- - - - <<?rCG|c"g5SWDKC#b'MCG#b3r7l2accO: :B<"$=,, , , , 199;BBBB;;&   <<3::r2   c            	      |   t        d      \  } }}t        dd      \  }}}t               \  }}}}	t        d||	| ||g|||g      }
|
j                  t	        | ||g      k(  sJ |
j
                  t	        |||g      k(  sJ |
j                  t	        g d      k(  sJ |j                  |      | t        |      z  t        |      z  |t        |      z  z
  |j                  z  |t        |      z  t        |      z  |t        |      z  z
  |j                  z  z   | t        |      z  |z
  |j                  z  z   k(  sJ y )Nzq0:3r>   r   )rw   rU   )r   r   r   )r   r1   r   rw   r	   rU   rx   r   r   r   r+   r,   r}   )rH   rI   rb   rL   rM   rd   r!   r"   r#   r&   r   s              r0   /test_spherical_joint_speeds_as_derivative_termsr2    s>     'JBB*JBB!JAq!QsAqr2rlBB<PA==FBB<000088vr2rl++++66VI&&&&<<?sSW}s2w6c"gEL
SWs2wc"g-P5 5sSW}r!QSS8)) ) ) )r2   c                    	
 t        d      \  } }}}}}t        d      \  t               \  }}
	t        d
	gg      }|j                  t	        g      k(  sJ |j
                  t	        g      k(  sJ t               \  }}
	t        d
	t	        g      t	        g            }|j                  t	        g      k(  sJ |j
                  t	        g      k(  sJ t               \  }}
	t        t        	
fd       t        t        	
f	d       t        t        	
f	d       y )Nr*  z
q0:4, u0:4r   c            	      L    t        d t        g      t        g            S Nr   r   r	   )r&   r#   rH   rI   rL   s   r0   r:   z-test_spherical_joint_coords.<locals>.<lambda>	  s#    >#q!VRH-=vrd|L r2   c                  T   	 t        d t        g      t        g            S r5  r6  )	r&   r#   rH   rI   rb   rc   rL   rM   rd   s	   r0   r:   z-test_spherical_joint_coords.<locals>.<lambda>  s1    ~Q62r2r*+VRRL-A C r2   c                  T   	 t        d t        g      t        g            S r5  r6  )	r&   r#   rH   rI   rb   rL   rM   rd   u4s	   r0   r:   z-test_spherical_joint_coords.<locals>.<lambda>  s1    ~Q62r2,'RR0@)A C r2   )r   r1   r   rw   r	   rU   r   rV   )q0sq1sq2su0su1su2sr!   r"   r   r&   r#   rH   rI   rb   rc   rL   rM   rd   r9  s            @@@@@@@@@@r0   test_spherical_joint_coordsr@    sb   #12B#C Cc3S%3L%A"BBBB!JAq!QsAq2r2,R=A==FBB<000088vr2rl++++!JAq!QsAq&"b""6r2rl+	-A==FBB<000088vr2rl++++!JAq!Q
:LN : C C D
: C C Dr2   c                  |   t        d      \  t        t               t               t              z  t              t              z  gt               t              z  t              t              z  t              z  t              t              z  z
  t               t              z  t              z  t              t              z  z
  gt              t              z  t               t              z  t              t              z  t              z  z
  t               t              z  t              t              z  t              z  z   gg      } t         t              z  z
  g t              z  t              z  t              z  z   gt              z  t              z  t              z  z   gg      }t        t	        d       t        t
        dz  z         z  t              z  t        t
        dz  z         z  z   z  g t              z  z
  g t              z  z
  gg      }t        d      \  }}}}}}t        d||gg|j                  |j                  z   |j                   |j                  z   ||dd      }	|	j                  j                         d	k(  sJ |	j                  dk(  sJ t        |j                  |      | z
        t!        d
      k(  sJ t        |j#                  |      j%                  |      |z
        t!        d
d      k(  sJ t        |j&                  j)                  |      j%                  |            |k(  sJ t        d      \  }}}}}}t        d||gg|j                  |j                  z   |j                   |j                  z   ||d	fd      }	fd}
|	j                  j                         d	k(  sJ |	j                  dk(  sJ t        |j                  |       |
|       z
        t!        d
      k(  sJ t        |j#                  |      j%                  |       |
|      z
        t!        d
d      k(  sJ t        |j&                  j)                  |      j%                  |             |
|      k(  sJ t        d      \  }}}}}}t        d||gg|j                  |j                  z   |j                   |j                  z   ||dd      }	|	j                  j                         d	k(  sJ |	j                  dk(  sJ t        |j                  |      t        t               t              z  t              t              z  t              z  t              t              z  z
  t              t              z  t              z  t              t              z  z   gt               t               t              z  t               t              z  gt              t              z  t               t              z  t              t              z  t              z  z
  t              t              z  t              t              z  t              z  z
  gg      z
        t!        d
      k(  sJ t        |j#                  |      j%                  |      t        t              z  z
  gt              z  t              z  t              z  z
  gt              z  t              z  t              z  z   gg      z
        t!        d
d      k(  sJ t        |j&                  j)                  |      j%                  |            t        t	        d       t        t
        dz  z         z  t              z  t        t
        dz  z         z  z   z  gt              z  z
  gt              z  z
  gg      k(  sJ y )Nr  r)   r   Tr   bodyr,  rw   rU   ro   rp   rr   ru   rot_type	rot_orderr+  r@   r>   	rw   rU   ro   rp   rr   ru   rD  amountsrE  c                 >    | j                  i      S r   r   exprrH   rI   rb   rL   rM   rd   s    r0   r:   z2test_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}   r-  upperr.  r
   r   r   r   r   rz   r   r   N_w_AN_v_Cor!   r"   r#   r&   r.   r/   r   switch_orderrH   rI   rb   rL   rM   rd   s              @@@@@@r0    test_spherical_joint_orient_bodyrT    s   +L9BBB
b'CG8c"g%s2wR'89
b'CG	SWs2w.R83r7SW;LL
b'CG	c"g	%B#b'(9	9	;	R3r7	SWHs2w.R3r71BSW1LL
b'CG	c"gB/#b'9	9	;	< =E bS3r7]R'(S3r7]SW,rCG|;<#b'\CG+b3r7l:;= >E 
q'R#b26k**SW4rCR!V<L7LL	MN
s2w	sSW}r124 5F ,D1Aq!QdsAqr2rlBB<$%CC!##IACC4!##:)- &#	7A ;;&(((<<3AEE!Hu$%q111ALLO--a0589U1a[HHHALL$$Q'11!45???+D1Aq!QdsAqr2rlB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Aq!QdsAqr2rlBB<$%CC!##IACC4!##:)- &%	9A ;;&(((<<5   AEE!Hv
b'CG	SWs2w.R83r7SW;LL	R3r7	SW	$s2wR'8	8	:
b'CG8c"g%Bx#b''9:	R3r7	SWHs2w.R3r71BSW1LL	R3r7	SWs2w.R8	8	:	';  < < =
 AFaI I I 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                     t        d      \  t        t               t              z  t              t              z  t              z  z
  t              t              z  t              z  t              t              z  z
  t              t              z  gt               t              z  t              t              z  t              z  z   t               t              z  t              z  t              t              z  z
  t               t              z  gt              t              z  t               t              z  t              gg      } t        t              z  t              z  t              z  z
  gt              z  t              z  t              z  z   gt              z  z
  gg      }t        t              z  z
  gt              z  z
  gt	        d       t        t
        dz  z         z  t        t
        dz  z         z  t              z  z   z  gg      }t        d      \  }}}}}}t        d||gg|j                  |j                  z   |j                   |j                  z   ||dd      }	|	j                  j                         d	k(  sJ |	j                  dk(  sJ t        |j                  |      | z
        t!        d
      k(  sJ t        |j#                  |      j%                  |            |k(  sJ t        |j&                  j)                  |      j%                  |            |k(  sJ fd}
t        d      \  }}}}}}t        d||gg|j                  |j                  z   |j                   |j                  z   ||d	fd      }	|	j                  j                         d	k(  sJ |	j                  dk(  sJ t        |j                  |       |
|       z
        t!        d
      k(  sJ t        |j#                  |      j%                  |             |
|      k(  sJ t        |j&                  j)                  |      j%                  |             |
|      k(  sJ t        d      \  }}}}}}t        d||gg|j                  |j                  z   |j                   |j                  z   ||dd      }	|	j                  j                         d	k(  sJ |	j                  dk(  sJ t        |j                  |      t        t               t              z  t               t              z  t              t              z  t              z  z   t              t              z  t              z  t              t              z  z   gt               t               t              z  t               t              z  gt              t              z  t               t              z  t              t              z  t              z  z
  t               t              z  t              z  t              t              z  z   gg      z
        sJ t        |j#                  |      j%                  |      t         t              z  z   g t              z  t              z  t              z  z   gt              z  t              z  t              z  z   gg      z
        t!        d
d      k(  sJ t        |j&                  j)                  |      j%                  |      t        t              z  t              z  t              z  z   gt              z  t              z  t              z  z   gt              z  z   t              z  z
  t              z  t              z  z
  gg      z
        t!        d
d      k(  sJ y )Nr  r)   r   Tr   spacer,  rC  SPACEr@   c                 >    | j                  i      S r   rI  rJ  s    r0   r:   z3test_spherical_joint_orient_space.<locals>.<lambda>h  rL  r2   rF  SPaCezxyr>   )r   r	   r   r   r   r   r1   r   r+   r}   r,   r-  rO  r.  r
   r   r   r   r   rz   r   rP  s              @@@@@@r0   !test_spherical_joint_orient_spacer[  N  s   +L9BBB
b'CG	c"gB/#b'9	9	R3r7	SW	$s2wR'8	8#b'CG:K	M
b'CG	c"gB/#b'9	9
b'CG	c"g	%B#b'(9	9CG8c"g;M	O	R3r7	SWHs2w.B8: ;E 	c"gSWs2w.	./	c"gSWs2w.	./"rCG|2C1DF GE 	b3r7l	b2B</0	aRC#b26k**R#b26k2B-BSW-LL	MNP QF ,D1Aq!QdsAqr2rlBB<$%CC!##IACC4!##:)- '3	8A ;;')))<<3AEE!Hu$%q111ALLO--a01U:::ALL$$Q'11!45???: :L+D1Aq!QdsAqr2rlB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Aq!QdsAqr2rlBB<$%CC!##IACC4!##:)- '5	:A ;;')))<<5   AEE!Hv
b'CG	c"gXB/#b'CG2Cc"g2MM	R3r7	SW	$s2wR'8	8	:
b'CG8c"g%Bx#b''9:	R3r7	SWHs2w.R3r71BSW1LL
b'CG	c"g	%B#b'(9	9	;	'<  = = > > > ALLO--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                  	   t               \  } } }}t        d||      }|j                  dk(  sJ |j                  |k(  sJ |j                  |k(  sJ |j
                  t               k(  sJ |j                  t               k(  sJ |j                  t        ddg       j                  k(  sJ |j                  j                  |j                        t        d      k(  sJ |j                  j                  |j                        t!        d      k(  sJ |j"                  j                  |j                        t!        d      k(  sJ |j"                  j                  |j                        t!        d      k(  sJ |j                  j                  |j                        t!        d      k(  sJ |j                  j%                  |j                        t!        d      k(  sJ |j                  j'                  |j                        dk(  sJ |j                  j'                  |j                        dk(  sJ |j)                         dk(  sJ t               \  }}}}t+        d      \  }}t-        d      }|j/                  |j                  |j0                  t2        dz         t        d||||j                  j4                  z  ||j                  j0                  z  |	      }|j                  j                  |j                        ||j                  j0                  z  k(  sJ |j"                  j                  |j                        ||j                  j4                  z  k(  sJ |j"                  j                  |j                        t!        d      k(  sJ |j                  j                  |j                        | |j4                  z  ||j0                  z  z   k(  sJ |j                  j%                  |j                        t!        d      k(  sJ |j                  j%                  |      t!        d      k(  sJ |j                  j'                  |j                        dk(  sJ |j                  j'                  |j                        dk(  sJ |j4                  |j6                  k(  sJ t9               5  t;        ||       d d d        y # 1 sw Y   y xY w)
NWr>   r   r@   z!WeldJoint: W  parent: P  child: Cri   r'   r)   )ro   rp   rr   )r1   r   rv   r4   r5   rw   r	   rU   rx   Tr%   r   r   rp   ry   rz   r   ro   r   r   r|   r   r   r*   r,   r   r+   r}   r   r   )	rf   r#   r&   r]  r!   r"   r   r   r.   s	            r0   test_weld_jointr_    s   !JAq!Q#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;====!JAq!Q5>DAq'"DQWWacc26*#q!!aggii-!''))mt	EA ==!!!,,/1qwwyy=@@@>>""1<<0A		MAAA>>""1==1VAY>>><<  .A#)a!##g2EEEE<<AGG$q	111<<D!VAY...77agg&!+++77agg&!+++33!##::		! Q  s   *S  S	c            
      8   t        d      \  } }t               \  }}}}t               5  t        d|||j                          d d d        |j                   j                  |j                        dk(  sJ |j                  j                  |       |j                  k(  sJ |j                  |      t        g ddt        |        t        |        gdt        |        t        |       gg      k(  sJ |j                  |      ||j                  z  k(  sJ |j                  |      j                         t        |dz        k(  sJ t               \  }}}}t               5  t        d|||j                  |j                  z          d d d        |j                  j                  |j                  |j                  z         dk(  sJ |j                  j                  |      |j                  |j                  z   t        d      z  k(  sJ |j                  |      t        t        d      dz  t        d      dz  dgt        d       dz  t        d      dz  dgg dg      k(  sJ |j                  |      t!        d      k(  sJ y # 1 sw Y   AxY w# 1 sw Y   xY w)	Nrj   r8   )
child_axisr   r   r)   )parent_axisr   )r   r1   r   r   r+   r   r   r   r	   r   r   r   r   r   r   r,   r   )rC   rA   r!   r"   r#   r&   s         r0   !test_deprecated_parent_child_axisrc    sD   *%DAq!JAq!Q		! -at,-SSD$)))CCKKN?acc!!!558vz !CF7SVG4 !CF7CF35 6 6 6 6 <<?a!##g%%%<<?$$&$qAv,666!JAq!Q		! 9sAqaccACCi89CCqssQSSy)Q...33;;q>accACCi472222558vQ!T!Wq[!<!%a1d1gk1=yJ K K K K<<?fQi'''#- -9 9s   J'JJJc            	         t               \  } }}}t               5  t        d||| j                  | j                  z   |j                  |j
                  z
        }d d d        j                  j                  |j                        | j                  | j                  z   k(  sJ |j                  j                  |j                        |j                  |j
                  z
  k(  sJ t               \  } }}}t               5  t        d||| j
                  | j                  z   |j                  |j                  z
        }d d d        j                  j                  |j                        | j
                  | j                  z   k(  sJ |j                  j                  |j                        |j                  |j                  z
  k(  sJ y # 1 sw Y   oxY w# 1 sw Y   xY w)Nr8   )parent_joint_poschild_joint_pos)r1   r   r   r+   r,   r}   ro   ry   rz   rp   r   )r!   r"   r#   r&   r   sliders         r0   test_deprecated_joint_posrh    sp   !JAq!Q		! 2sAq1339'(ssQSSy22 $$Q\\2accACCi?????##ALL1QSS133Y>>>!JAq!Q		! ;QACC!##I01acc	;; ''5qssBBB&&q||4acc	AAA2 2; ;s   ?G<?GGGN)F)Esympy.core.functionr   sympy.core.numbersr   sympy.core.singletonr   (sympy.functions.elementary.miscellaneousr   (sympy.functions.elementary.trigonometricr   r   sympyr	   r
   r   r   sympy.core.symbolr   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   sympy.physics.mechanics.jointr   sympy.physics.vectorr   r   r   sympy.testing.pytestr   r   _trX   r1   r<   rg   r   r   r   r   r   r   r   r   r   r   r  r  r  r  r  r#  r(  r0  r2  r@  rT  r[  r_  rc  rh   r2   r0   <module>rv     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   