Ë
    7^(hBâ  ã                   óŽ  — 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          úf/var/www/skyplay_api_hub/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Ó,¬n¸WÓ.EˆdˆØ×Ñ˜˜AŸC™C¤Ô$Ø×Ñ˜˜AŸC™C¤"  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   ø€ œe C¨°Ó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¸¸B°xÀÓ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Ñ5°r¼7À4»=Ð6IÈ1Ó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Ñ5°r¸B±wÀ°mÀQÓG€ r2   c                  ó&   •— t        d‰‰ ‰d d g«      S r7   ©r   )r&   r#   Úq1js   €€€r0   r:   z,test_coordinate_generation.<locals>.<lambda>Q   s   ø€ œ~¨c°1°a¸#¸tÀTÐ9JÓK€ r2   c                  ó(   •— t        d‰‰ ‰‰‰g¬«      S )Nr8   )ÚspeedsrR   )r&   r#   rL   rM   s   €€€€r0   r:   z,test_coordinate_generation.<locals>.<lambda>R   s   ø€ œ~¨c°1°aÀÀ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  ÿ€ Ü! -Ó0�L€A€qˆ"ˆbÜ-;¸OÓ-LÑ*€Cˆˆc�3˜˜S # sÜ%3°KÓ%@Ñ"€BˆˆB��B˜˜B ÜÓ!�J€A€qˆ!ˆQä��a˜Ó€Aà×"Ñ" 1 aÓ(¬F°A°3«KÒ7Ð7Ð7Ø×"Ñ" A 3¨Ó*¬f°a°S«kÒ9Ð9Ð9Ø×"Ñ" A 3¨°!Ð"Ó4¼À¸s»ÒCÐCÐCà×"Ñ" 4¨Ó+¬v°r°d«|Ò;Ð;Ð;Ø×"Ñ" D 6¨1Ó-´¸¸³Ò=Ð=Ð=Ø×"Ñ" B¨¨dÐ#3°QÓ7¼6Ø	ˆS�#ˆó<ò ð ð ð ×"Ñ" 4¨Ó+¬v°s¸CÀ°oÓ/FÒFÐFÐFØ×"Ñ" 2 qÓ)¬V°S¸#¸s°OÓ-DÒDÐDÐDà×"Ñ" 2 q°Ð"Ó3´v¸sÀCÈ¸oÓ7NÒNÐNÐNØ×"Ñ" B¨¨b >°1¸QÐ"Ó?Ä6Ø	ˆS�"ˆóDò ð ð à×"Ñ" 4¨°1Ð"Ó5¼ÀÀcÀ
Ó9KÒKÐKÐKà×"Ñ" 4¨¨CÓ0´F¸B¸4³LÒ@Ð@Ð@Ø×"Ñ" 2 q¨#Ó.´&¸#¸sÀC¸Ó2IÒIÐIÐIà×"Ñ" 4¨¸$Ð"Ó?Ä6È3È%Ã=ÒPÐPÐPØ×"Ñ" 2 q¨#¨q°$Ó7¼6À3À%»=ÒHÐHÐHØ×"Ñ" 2 q¨#Ó.´&¸#¸sÀC¸Ó2IÒIÐIÐIä
Œ:ÕCÔDÜ
Œ:ÕNÔOÜ
Œ:ÕCÔDä
Œ9ÔMÔNÜ
Œ9ÕGÔHäÓ!�J€A€qˆ!ˆQÜˆS�!�Q˜˜BŸG™G¤A›JÔ'äÓ!�J€A€qˆ!ˆ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‹>�D€A€qÜ˜*Ó%�D€A€qÜ	�#�q˜!Ó	€BØ�7‰7�cŠ>Ðˆ>Ø�9‰9˜Š>Ðˆ>Ø�8‰8�qŠ=Ðˆ=Ø�>‰>œV Q C›[Ò(Ð(Ð(Ø�9‰9œ ˜s›Ò#Ð#Ð#Ø�7‰7”f˜a !§&¡&¬£)™m˜_Ó-Ò-Ð-Ð-Ø�=‰=˜AŸG™GŸI™IÒ%Ð%Ð%Ø�>‰>×"Ñ" 1§<¡<Ó0´F¸1³IÒ=Ð=Ð=Ø�?‰?×#Ñ# A§L¡LÓ1´V¸A³YÒ>Ð>Ð>Ø�?‰?×#Ñ# B§O¡OÓ4¼¸q»	ÒAÐAÐAØ�<‰<× Ñ  §¡Ó.´&¸³)Ò;Ð;Ð;Ø×Ñ 1§7¡7Ò*Ð*Ð*Ø×Ñ !§'¡'Ò)Ð)Ð)Ø�:‰:‹<Ð=Ò=Ð=Ð=ä	�4‹€BÜ	�4‹€BÜ˜'Ó"€DØ×Ñ�R—X‘X˜rŸt™t¤R¨!¡VÔ,Ü	�$˜˜B¨Q¨r¯x©x¯z©z©\Ø §¡§
¡
™l°r·x±x·z±zØ$(ô
*€Bð �>‰>˜RŸX™XŸZ™ZÒ'Ð'Ð'Ø�?‰?×#Ñ# B§M¡MÓ2°a¸"¿(¹(¿*¹*±nÒDÐDÐDØ×Ñ×$Ñ$ R§]¡]Ó3°q¸2¿8¹8¿:¹:±~ÒEÐEÐEØ×Ñ×$Ñ$ R§_¡_Ó5¼À»ÒBÐBÐBØ�M‰M×"Ñ" 2§=¡=Ó1ØˆBˆr�x‰x�z‰z‰M˜A˜bŸh™hŸj™j™LÑ(ò)ð *ð )à×Ñ 4Ò'Ð'Ð'Ø×Ñ "§(¡(Ò*Ð*Ð*ä˜&Ó!�D€A€qÜ+¨DÓ1Ñ€A€qˆ!ˆQ��dØ—<‘<×)Ñ)¨.¸!¿#¹#ÀÇÁ¹)ÓD€LØ—,‘,×(Ñ(¨¸¿¹¸a¿c¹c¹	ÓB€KÜ��a˜˜A˜q¨|Ø(¸DØ"&°1·3±3ô	8€Að �<‰<˜1Ÿ3™3ÒÐÐØ�>‰>×Ñ˜aÓ  AÒ%Ð%Ð%Ø�>‰>˜\Ò)Ð)Ð)Ø�=‰=˜KÒ'Ð'Ð'Ø�=‰=×!Ñ! !§,¡,Ó/°1·3±3¸¿¹±9Ò<Ð<Ð<Ø�>‰>×"Ñ" 1§<¡<Ó0°A·C±C¸!¿#¹#±IÒ=Ð=Ð=Ø�<‰<× Ñ  §¡Ó.°!·#±#¸¿¹±)¸a¿c¹c±/ÀAÇCÁCÑ2GÒGÐGÐGØ�<‰<×Ñ˜AÓ×&Ñ& qÓ)¨a´#°a³&©j¸1¼sÀ1»v¹:Ñ.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‹>�D€A€qÜ˜SÓ!€GÜ�‹€AÜ�˜1Ô€AÜ˜&Ó!�D€A€qÜ��a˜˜A˜q°7Ø §¡™]ô	,€Aà×Ñ Ò(Ð(Ð(Ø×Ñ×#Ñ# {Ò2Ð2Ð2Ø�<‰<× Ñ  §¡Ó.°1°"°w·y±y±.Ò@Ð@Ð@Ø�;‰;�q×*Ñ*Ó+¬v²yØ89¼3¸q»6Ä3ÀqÃ6Ð7JØ89¼CÀ»F¸7ÄCÈÃFÐ7Kð7Mó 0Nò Nð Nð Nð �<‰<×Ñ˜A×/Ñ/Ó0°Q°B¸±F¸W¿Y¹YÑ4FÒFÐFÐFä˜SÓ!€GÜ˜SÓ!€GÜ�‹€AÜ�˜1Ô€AÜ˜&Ó!�D€A€qÜ��a˜˜A˜q°GØ")°q¸7¿9¹9±}Ø#ŸI™Iô	'€Að �<‰<˜1×.Ñ.×0Ñ0Ò0Ð0Ð0Ø�;‰;�q×*Ñ*Ó+¬v¼¸A»ÄÀAÃÈÐ7JÜ9<¸Q»¸ÄÀQÃÈÐ7KÚ7@ð7Bó 0Cò Cð Cð Cð �<‰<×Ñ˜A×/Ñ/Ó0°AÒ5Ð5Ð5Ø�<‰<×Ñ˜A×/Ñ/Ó0°A¸±E¸G¿I¹IÑ4EÒEÐEÐEÜ+¨KÓ8Ñ€BˆˆB��B˜Ø02°B¨x¸"¸b¸ÀBÈÀ8Ð.L×M¡F B¨�—‘œ“˜R‘ÐM€IÑMä�C‹=œ( 3›-€q€AÜ�s˜A˜q ! QÓ'€AØ×Ñ×"Ñ" 1×#5Ñ#5Ó6¼#¸a»&Ò@Ð@Ð@Ø�<‰<× Ñ  §¡Ó.°!°a×6IÑ6I×6KÑ6KÑ2KÒKÐKÐKØ�<‰<×Ñ˜A×/Ñ/Ó0°AÒ5Ð5Ð5Ø�<‰<×Ñ˜A×/Ñ/Ó0°A¸×8KÑ8K×8MÑ8MÑ4MÒMÐMÐMä�C‹=œ( 3›-€q€AÜ˜YÓ'€GÜ˜˜a  B¨¨B°ÀgØ&'¨'¯)©)¡mÀÇ	Á	ô	K€Aà×Ñ×"Ñ" 1×#5Ñ#5Ó6¼&Ü	ˆR‹�!”S˜“WÐšy¬C°«G¨8°Q¼¸B»Ð*@ðBBó ;Cò Cð Cð Cà�<‰<× Ñ  §¡Ó.°!°g·i±i±-À"ÀwÇyÁyÁ.Ñ2PÒPÐPÐPØ�<‰<×Ñ˜A×/Ñ/Ó0°B¸¿¹±NÒBÐBÐBØ�<‰<×Ñ˜A×.Ñ.Ó/×8Ñ8¸ÓCØ	ˆˆg�i‰i‰˜!˜b™& 7§9¡9Ñ,Ñ,ò.ð .ð .ô �C‹=œ( 3›-€q€AÜ˜YÓ'€GÜ�C˜˜A˜r B¨ 8¨R°"°b°Ø%,¸!¸g¿i¹i¹-ô	I€Aà×!Ñ!€GØ×Ñ×"Ñ" 1×#5Ñ#5Ó6¼&Ú�A”s˜2“w¤ R£ Ð)¨A¬s°2«w¼¸B»Ð+@ðBBó ;Cò Cð Cð Cà�<‰<× Ñ  §¡Ó.Ø	
ˆˆW�Y‰Y‰˜˜gŸi™i™Ñ'¨"¨w¯y©y©.Ñ8ò:ð :ð :à�<‰<×Ñ˜A×/Ñ/Ó0Ø	ˆB‰�—‘Ñ˜R '§)¡)™^Ñ+¨b°7·9±9©nÑ<ò>ð >ð >ô �C‹=œ( 3›-€q€AÜ% iÓ0´.ÀÓ2KˆW€GÜ�#�q˜!¨wØ#*¸¸W¿Y¹Y¹Ø '§)¡)™mô	-€Að �;‰;�wÓ¤3 q£6Ò)Ð)Ð)Ø�<‰<× Ñ  §¡Ó.°!°g·i±i±-À!ÀgÇiÁiÁ-Ñ2OÒOÐOÐOØ�<‰<×Ñ˜A×/Ñ/Ó0°AÒ5Ð5Ñ5ùóO 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‹>�D€A€qÜ˜SÓ!€GÜ	Ó	!ñ -Ü�‹IˆÜ�˜1 GÔ,ˆ÷-ô ˜&Ó!�D€A€qÜˆS�!�Q˜˜1¨!¨g¯i©i©-Õ8Ø�7‰7�gÒÐÐØ�7‰7�<‰<˜9Ò$Ð$Ð$Ø�<‰<× Ñ  §¡Ó.°1°"°q·s±s±(Ò:Ð:Ð:Ø�7‰7�;‰;�q—w‘wÓ¤6ª9Ø,-¬s°1«v´s¸1³vÐ+>Ø,-´°A³¨w¼¸A»Ð+?ð+Aó $Bò Bð Bð Bð �<‰<×Ñ˜AŸG™GÓ$¨¨¨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Ó$�F€BˆÜ˜GÓ$�F€BˆÜ�5‹>�D€A€qÜ�sÓ€AÜ�sÓ€AÜ�sÓ€AÜ�#˜QÔ€AÜ�c ¨Ô+€EÜ�c ¨Ô+€Eä	�$˜˜5¨¸Ø ˜b §¡™f°·±·±ô
<€Bä	�$˜˜u¨R¸RØ ˜b §¡™f°·±·±ô
@€Bð �5‰5�‹8”v¤ B£¬#¨b«'¨°1Ð5Ü # B£¬¨R«°!Ð4²iðAó Bò Bð Bð Bà�5‰5�‹8”v¤ B£¬#¨b«'¨°1Ð5Ü # B£¬¨R«°!Ð4²iðAó Bò Bð Bð Bä�A—E‘E˜!“HÓ¤¬#¨b°2©g«,¼¸RÀ"¹W»¸ÀqÐ)IÜ25°b¸2±g³,ÄÀBÈÁGÃÈaÐ1PÚ1:ð)<ó "=ò =ð =ð =ð
 �<‰<˜‹?˜b 1§3¡3™hÒ&Ð&Ð&Ø�<‰<˜‹?˜b 1§3¡3™hÒ&Ð&Ð&Ø�<‰<˜‹?˜b 1§3¡3™h¨¨a¯c©c©Ñ1Ò1Ð1Ð1ð �7‰7”f˜b 2§7¡7¬1£:™oÐ.Ó/Ò/Ð/Ð/Ø�7‰7”f˜b 2§7¡7¬1£:™oÐ.Ó/Ò/Ð/Ð/ð ×Ñ×Ñ Ó" a¨¡d¨1¯3©3¡hÒ.Ð.Ð.Ø×Ñ×Ñ Ó" a¨¡d¨1¯3©3¡hÒ.Ð.Ð.Ø×Ñ×Ñ Ó" a¨¡d¨1¯3©3¡h°°B¸±G±¸Q¿S¹S±Ñ&@Ò@Ð@Ñ@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Ñ€Eˆ3��uÜ�sÓ€AÜ�sÓ€AÜ�sÓ€AØ
ˆq�1‰u‰*˜Ñ	€BØ
ˆQ‰$��1‘‰*€BÜ
�E ¨Ô
,€CÜ�g B¨aÔ0€EÜ�#˜QÔ€AÜ	�$˜˜3¨E¸%Ø  §¡™f°·±ô
6€Bä	�$˜˜U°¸EØ! !§#¡#™v°!·#±#ô
7€Bð �5‰5�‹8”v¤ E£
¨A´°E³
¨{Ð;Ú(Ü # E£
¨A¬s°5«zÐ:ð<ó =ò =ð =ð =ð �5‰5�‹8”v¤ C£¬3¨s«8¨)°QÐ7Ü # C£¬#¨c«(°AÐ6Ú(ð*ó +ò +ð +ð +ð �5‰5�‹8”vÜ	ˆS‹”#�e“*Ñ	œc #›h¬¨U«¨´C¸³HÑ(<Ð=Ü
ˆc‹(ˆ”3�u“:Ñ	œs 3›x¬¨S«´#°e³*Ñ)<Ð=Ü	ˆU‹�Qœ˜E›
Ð#ð%ó &ò &ð &ð &ð �<‰<˜‹?˜e A§C¡C™iÒ'Ð'Ð'Ø�<‰<˜‹?˜u˜f Q§S¡S™jÒ(Ð(Ð(Ø�<‰<˜‹?˜u˜f Q§S¡S™j¨5°·±©9Ñ4Ò4Ð4Ð4ð �7‰7”f˜e e§j¡j´£mÑ3Ð4Ó5Ò5Ð5Ð5Ø�7‰7”f˜e c§h¡h¬q£kÑ1Ð2Ó3Ò3Ð3Ð3ð �<‰<× Ñ  §¡Ó0°B°q·s±s±FÒ:Ð:Ð:Ø�>‰>×"Ñ" 5×#3Ñ#3Ó4¸"¸¸q¿s¹s¹
ÒBÐBÐBð �>‰>×Ñ˜aÓ  Q q¡S¨2¨a©4¡Z°Ñ$6°q·s±sÑ$:Ò:Ð:Ð:Ø×Ñ×Ñ Ó"¨¨!©¨b°©d©
°EÑ'9Ø()¨!©¨b°©d©
°EÑ'9ñ(:Ø;<¿3¹3ñ'?ò ?ð ?ñ ?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#	  û€ Ü˜&Ó!�D€A€qäÓ!�J€A€qˆ!ˆQÜ Ó'¬¸Ó)?€J€Dˆ$Ü
Œ:ÕJÔKÜ
Œ:ÕIÔJà×Ñ�Q˜Ÿ™˜QÔØ×Ñ�Q˜Ÿ™˜QÔÜ
Œ:ÕJÔKÜ
Œ:ÕIÔJäÓ!�J€A€qˆ!ˆQÜ˜&Ó!€DØ×Ñ˜1œr A™v¤r¬2°©6Ð2°EÔ:ÜˆS�!�Q˜˜1¨1¯3©3¸Q¿S¹S¸DØ#°·±õ8ä�A—E‘E˜!“HÓ¤Ø	”�a“œ3˜q›6Ñ! AÑ%¬¨Q« x´#°a³&Ñ'8¸1Ñ'<Ð=Ü	ˆa‹�1‰”d˜1“g ¤S¨£V¡¬c°!«fÑ!4Ñ5¸Ñ9Ü	ˆa‹”S˜“V�G˜a¤# a£&™jÑ(Ñ	)¨AÑ	-ð	/ä	ˆa‹�1‰”d˜1“g ¤c¨!£f¡¬s°1«vÑ!5Ñ6¸Ñ:Ü
ˆq‹'ˆ”S˜“V˜a¤# a£&™jÑ(Ñ	)¨AÑ	-ð	/ð	(0ó !1ñ 1ô
 5:¸!³Hò=ð =ð =ð �<‰<˜‹?˜a $§&¡&™jÒ(Ð(Ð(Ø�<‰<× Ñ  §¡Ó.°!·#±#¸¿¹±)Ò;Ð;Ð;Ø�<‰<×Ñ˜AÓ ! a§c¡c¡'Ò)Ð)Ð)Ø�<‰<×Ñ˜DÓ!¤V¨A£YÒ.Ð.Ð.Ø�<‰<×Ñ˜DÓ! Q¨¯©¡WÒ,Ð,Ð,äÓ!�J€A€qˆ!ˆQÜ˜&Ó!€DØ×Ñ˜1˜q¤2™v¨™z¬B¨3´°Q±Ð7¸Ô?ÜˆS�!�Q˜˜1¨A¯C©C¨4¸Q¿S¹SØ"¨q¯s©s°Q·S±S©yõ:ä�A—E‘E˜!“HÓ¤Ü
ˆq‹'ˆ”C˜“FÑ	˜QÑ	Ü
ˆq‹'ˆ”S˜“V˜a‘ZÑ	  1Ñ	$¤s¨1£v°¡zÑ	1´A°a³D¸1±HÑ	<Ü	ˆa‹”C˜“F˜Q‘JÑ	 !Ñ	#¤c¨!£f¨q¡jÑ	0´1°Q³4¸!±8Ñ	;ð	=ô 
ˆQ‹”$�q“'œD ›GÑ#¬¨A« wÑ.°Ñ2Ü�‹7ˆ(”T˜!“WÑ
¤ A£Ñ	&¨Ñ	*ð	,ä	ˆa‹”3�q“6Ñ	˜AÑ	Ü	ˆa‹”C˜“F˜Q‘JÑ	 !Ñ	#¤c¨!£f¨q¡jÑ	0´1°Q³4¸!±8Ñ	;Ü	ˆa‹�Aœ˜A›‘JÑ	 !Ñ	#¤c¨!£f¨q¡jÑ	0´1°Q³4¸!±8Ñ	;ð	=ð)>ó "?ò ?ð ?ð ?ð �<‰<˜‹?œd 1›g¨™k¨A™o°·±Ñ3´d¸1³gÀ±kÀA±oÈÏÉÑ6KÑKÒKÐKÐKØ�<‰<× Ñ  §¡Ó.°A·C±C°%¸!¿#¹#±+Ò=Ð=Ð=Ø�<‰<×Ñ˜AÓ×'Ñ'Ó)Ü	ˆa‹ˆ”4˜“7Ñ˜añ. Ø"#ñ.$Ø&'§c¡cñ.*ä�Q“ˆxœ$˜q›'Ñ! Qñ-'Ø)*ñ-+Ø-.¯S©Sñ-1ñ.1ò 1ð 1ð 1ð �<‰<×Ñ˜DÓ!¤V¨A£YÒ.Ð.Ð.äÓ!�J€A€qˆ!ˆQÜ Ó'¬¸Ó)?€J€Dˆ$Ø×Ñ˜1¤˜s Q™w¬¬B°©FÐ3°UÔ;Ø×Ñ˜1˜q¤2™v¨™z¬B¨3´°Q±Ð7¸Ô?ÜˆS�!�Q˜˜1¨1¯3©3°·±©9À1Ç3Á3À$Ø#°dØŸ™ §¡™õ)ô �A—E‘E˜!“HÓ¤Ü	ˆQ‹”$�q“'œD ›GÑ#¬¨A« wÑ.°Ñ2Ü�‹7ˆ(”T˜!“WÑ
¤ A£Ñ	&¨Ñ	*ð	,ä
ˆq‹'ˆ”C˜“FÑ	˜QÑ	Ü
ˆq‹'ˆ”S˜“V˜a‘ZÑ	  1Ñ	$¤s¨1£v°¡zÑ	1´A°a³D¸1±HÑ	<Ü	ˆa‹”C˜“F˜Q‘JÑ	 !Ñ	#¤c¨!£f¨q¡jÑ	0´1°Q³4¸!±8Ñ	;ð	=ô 
ˆa‹”3�q“6Ñ	˜AÑ	Ü	ˆa‹”C˜“F˜Q‘JÑ	 !Ñ	#¤c¨!£f¨q¡jÑ	0´1°Q³4¸!±8Ñ	;Ü
ˆq‹'ˆ”S˜“V˜a‘ZÑ	  1Ñ	$¤s¨1£v°¡zÑ	1´A°a³D¸1±HÑ	<ð	>ð)?ó "@ò @ð @ð @ð �<‰<˜‹?œd 1›g¨™k¨A™o°·±Ñ6¼Ø	ó:Øñ:Øñ:Ø—V‘Vñ:ñ ò ð ð à�<‰<× Ñ  §¡Ó.°!·#±#¸¿¹±)¸a¿c¹c±/ÒAÐAÐAÜ�1‹gˆXœ˜Q›Ñ 1Ñ$ qÑ(¨1¯3©3Ñ.€EØ�<‰<×Ñ˜AÓ×'Ñ'Ó)¨UÒ2Ð2Ð2Ø�<‰<×Ñ˜DÓ!×*Ñ*Ó,°Ò5Ð5Ð5Ø�<‰<×Ñ˜DÓ!¤V¨A£YÒ.Ð.Ñ.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°Ü36°q³6¸A¿C¹C±<Ä#ÀaÃ&È1Ï3É3Á,Ñ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°¸a¿c¹cÔ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°¸a¿c¹cÀAÇCÁC¹iÔ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� T¸DØ—6‘6˜AŸC™C‘<ô !€ 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� T¸DØ—3‘3˜Ÿ™‘<ô !€ 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� T¸DÜ˜!“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� T¸DØ—3‘3˜Ÿ™‘<ô!€ r2   )r   r1   r   r,   rq   r‘   r	   r   r   r}   r+   r   rV   Ú	Exception)	rA   r"   Úpinr&   r/   r!   r#   r.   rC   s	      @@@@@@r0   Útest_pin_joint_joint_axisrÜ   ‹  sA  ý€ Ü˜&Ó!�D€A€qä+¨DÓ1Ñ€A€qˆ!ˆQ��dÜ
�3˜˜1˜a °dØ$(°Q·S±Sô:€Cà�>‰>˜QŸS™SÒ Ð Ð Ø�5‰5�‹8”v¤ A£¨¬3¨q«6Ð2²JÜ # A£¨¬C°«F¨7Ð3ð5ó 6ò 6ð 6ð 6ô ,¨DÓ1Ñ€A€qˆ!ˆQ��dÜ
�3˜˜1˜a °dØ$(°T·V±Vô=€Cà�>‰>˜TŸV™VÒ#Ð#Ð#Ø�5‰5�‹8”v¤ Q£ ¨¬C°«FÐ3²ZÜ # A£¨¬3¨q«6Ð2ð4ó 5ò 5ð 5ð 5ô  Ó!�J€A€qˆ!ˆQÜ
�3˜˜1˜a °a·c±cØ%&§S¡S D°Q·S±Sô:€Cà�>‰>˜QŸS™SÒ Ð Ð Ø�5‰5�‹8”v¤ Q£ ¬#¨a«&¨°!Ð4¼¸A»°wÄÀAÃÈÐ6JÚ)ð+ó ,ò ,ð ,ð ,äÓ!�J€A€qˆ!ˆQÜ
�3˜˜1˜a °a·c±cØ%&§S¡S D°Q·S±Sô:€Cà�>‰>˜QŸS™SÒ Ð Ð Ø�5‰5�‹8”všz¨A¬s°1«v´s¸1³vÐ+>Ø !¤3 q£6¬C°«F¨7Ð3ð5ó 6ò 6ð 6ð 6ô ,¨DÓ1Ñ€A€qˆ!ˆQ��dÜ
Œ:ö Pô Qô Œ:ÔBÔCä
Œ:ÔHÔIÜ
Œ:ö !ô "ô Œ:ö !ô "ô ,¨DÓ1Ñ€A€qˆ!ˆ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  € Ü˜*Ó%�D€A€qô  Ó!�J€A€qˆ!ˆQÜˆS�!�Q¨!¯#©#¨Õ.à�S‰SˆD×Ñ §¡Ó$¨Ò)Ð)Ð)Ø�C‰C�K‰K˜‹Nˆ?˜aŸc™cÒ!Ð!Ð!Ø�5‰5�‹8”všzØ !¤C¨£F 7¬S°«V¨GÐ4Ø !¤C¨£F 7¬C°«FÐ3ð5ó 6ò 6ð 6ð 6ð �<‰<˜‹?˜a §¡™eÒ#Ð#Ð#Ø�<‰<˜‹?×$Ñ$Ó&¬$¨q°!©t«*Ò4Ð4Ð4Ø�<‰<× Ñ  §¡Ó.°!Ò3Ð3Ð3Ø�<‰<× Ñ  §¡Ó.×6Ñ6°qÓ9×BÑBÓDÈÒIÐIÐIØ�<‰<×Ñ˜AÓ !Ò#Ð#Ð#ô  Ó!�J€A€qˆ!ˆQÜˆS�!�Q¨¯©¸!¿#¹#Õ>à�3‰3×Ñ˜QŸS™SÓ! QÒ&Ð&Ð&Ø�3‰3�;‰;�q‹>˜QŸS™SÒ Ð Ð Ø�5‰5�‹8”v ¤C¨£F 7¬S°«V¨GÐ4Ú(Ø !¤C¨£F 7¬C°«FÐ3ð5ó 6ò 6ð 6ð 6ð �<‰<˜‹?˜a §¡™eÒ#Ð#Ð#Ø�<‰<˜‹?×"Ñ" 1Ó%¨¨Q¯S©S©Ò0Ð0Ð0Ø�<‰<˜‹?×$Ñ$Ó&¬$¨q°!©t«*Ò4Ð4Ð4Ø�<‰<˜‹?× Ñ  §¡Ó%¨Ò*Ð*Ð*Ø�<‰<×Ñ˜AÓ ! A§C¡C¡%Ò'Ð'Ð'Ø�<‰<× Ñ  §¡Ó.°1·3±3°$Ò6Ð6Ð6Ø�L‰L×!Ñ! !§,¡,Ó/×7Ñ7¸Ó:×CÑCÓEÜ�‹F�1—3‘3‰Jœ˜Q› §¡™Ñ#ò$ð %ð $à�<‰<×Ñ˜AÓ×,Ñ,¨Q¯S©SÓ1´R¸±TÒ9Ð9Ð9ô  Ó!�J€A€qˆ!ˆQÜˆS�!�Q¨!¯#©#¸A¿C¹CÕ@à�3‰3×Ñ˜QŸS™SÓ! QÒ&Ð&Ð&Ø�3‰3�;‰;�q‹>˜QŸS™SÒ Ð Ð Ø�5‰5�‹8”všyÜ!$ Q£ ¨¬C°«FÐ3Ü # A£¨¬3¨q«6Ð2ð4ó 5ò 5ð 5ð 5ð �<‰<˜‹?˜a §¡™eÒ#Ð#Ð#Ø�<‰<˜‹?×"Ñ" 1Ó%¨¨1¯3©3©Ò.Ð.Ð.Ø�<‰<˜‹?×$Ñ$Ó&¬$¨q°!©t«*Ò4Ð4Ð4Ø�L‰L˜‹O×)Ñ)¨!¯#©#Ó.€EØ�>‰>˜1˜a˜&Ó! QÒ&Ð&Ð&Ø�<‰<×Ñ˜AÓ !Ò#Ð#Ð#Ø�<‰<× Ñ  §¡Ó.°!·#±#Ò5Ð5Ð5ô  Ó!�J€A€qˆ!ˆQÜˆS�!�Q Q§S¡S°a·c±cØŸc™c A§C¡C™iõ)ä�a—c‘c×'Ñ'¨¯©¨a¯c©c©	Ó2Ó3°qÒ8Ð8Ð8Ø�C‰C�!—#‘#‰I×Ñ˜qÓ!×*Ñ*Ó,´°Q³¸¿¹±Ò;Ð;Ð;Ü�A—E‘E˜!“HÓ¤Ü	ˆa‹�‰”T˜!“W�HœS ›V‘O AÑ%¬¨Q« x´°A³¡°qÑ'8Ð9Ü	ˆa‹�‰”D˜“GœC ›F‘N 1Ñ$¤d¨1£g¬c°!«f¡n°QÑ&6Ð7Ø	
ŒS�‹VˆG”S˜“VÐð)ó "ò ð ð ð �<‰<˜‹?˜a §¡™eÒ#Ð#Ð#Ø�L‰L˜‹O×#Ñ# AÓ&×/Ñ/Ó1Øˆq�s‰s‰U�Q�q—s‘s‘U‰]œD ›GÑ#ò$ð %ð $à�<‰<˜‹?×$Ñ$Ó&¬$¨q°!©t«*Ò4Ð4Ð4Ø�L‰L˜‹O×)Ñ)¨!¯#©#°·±©)Ó4€EØ�>‰>˜1˜a˜&Ó! QÒ&Ð&Ð&Ø�<‰<×Ñ˜AÓ×'Ñ'Ó)¨a°!·#±#©g´t¸A³wÑ->Ò>Ð>Ð>Ø�<‰<× Ñ  §¡Ó.°!·#±#¸¿¹±)Ò;Ð;Ð;Ø�L‰L×!Ñ! !§,¡,Ó/×7Ñ7¸Ó:×CÑCÓEØ”�a“˜‘‰]˜AŸC™CÑ¤$ q£'¬#¨a«&¡.°Ñ"2°1·3±3Ñ"6Ñ6Ü�‹G”C˜“F‰N˜1Ñ˜QŸS™SÑ ñ!ò!ð "ð !ð �L‰L×Ñ˜QÓ×'Ñ'¨Ó*×3Ñ3Ó5Ü�!‹WˆH�Q‰J”s˜1“vÑ˜aÑ §¡Ñ#¤d¨1£g¨a¡i´°A³Ñ&6°qÑ&8¸¿¹Ñ&<Ñ<ò=ð >ð =à�<‰<×Ñ˜AÓ×,Ñ,¨Q¯S©SÓ1´R¸±TÒ9Ð9Ð9äÓ!�J€A€qˆ!ˆQÜˆS�!�Q Q§S¡S°a·c±cØŸc™c A§C¡C™i¨!¯#©#™oõ/ä�a—c‘c×'Ñ'¨¯©¨a¯c©c©	°A·C±C©Ó8Ó9¸QÒ>Ð>Ð>Ø�C‰C�!—#‘#‰I˜Ÿ™‰O×$Ñ$ QÓ'×0Ñ0Ó2´d¸1³g¸a¿c¹c±kÒAÐAÐAÜ�A—E‘E˜!“HÓ¤Ü	ˆa‹�‰”T˜!“W�HœS ¤R¨¡T¡›]Ñ*¨1Ñ,Ü	ˆa‹”�Qœ˜A™‘X“Ñ	˜qÑ	 ð	"ä	ˆa‹�‰”D˜“GœC ¤B r¡E¡	›NÑ*¨1Ñ,Ü	ˆa‹”�Qœ˜B™‘Y“Ñ	 Ñ	!ð	#ä
ˆq‹'ˆ�!‰”T˜!“WœS  Q¤r¡T¨"¡W¡Ó-Ñ-¨aÑ/Ü	ˆa‹”�Q˜œ2™˜b™‘[Ó!Ñ	! !Ñ	#ð	%ð)&ó "'ò 'ð 'ð 'ð �<‰<˜‹?˜a §¡™eÒ#Ð#Ð#Ø�<‰<˜‹?×"Ñ" 1Ó%×.Ñ.Ó0°Q°q·s±s±U¸Q¸q¿s¹s¹U±]Ø56°q·s±s±Uñ6;Ü<@À»Gñ5Dò Dð Dð Dà�<‰<˜‹?×$Ñ$Ó&¬$¨q°!©t«*Ò4Ð4Ð4Ø�L‰L˜‹O×)Ñ)¨!¯#©#°·±©)°A·C±C©-Ó8€EØ�>‰>˜1˜a˜&Ó!×*Ñ*Ó,°Ò1Ð1Ð1Ø�<‰<×Ñ˜AÓ×'Ñ'Ó)¨a°·±©e°a¸¿¹±e©m¼TÀ!»WÑ-DÒDÐDÐDØ�<‰<× Ñ  §¡Ó.°!·#±#¸¿¹±)Ò;Ð;Ð;Ø�L‰L×!Ñ! !§,¡,Ó/×7Ñ7¸Ó:×CÑCÓEØ”�a“˜‘‰]˜AŸC™CÑ¤$ q£'¬#¨a´"°Q±$©h«-Ñ"7¸Ñ"9¸!¿#¹#Ñ"=Ñ=Ü�‹G”C˜œB˜q™D™“MÑ! !Ñ# A§C¡CÑ'ñ(ò(ð )ð (ð �L‰L×Ñ˜QÓ×'Ñ'¨Ó*×3Ñ3Ó5Ü�‹G�A‰I”c˜!œb ™d™(“mÑ# AÑ% a§c¡cÑ)Ü�‹G�A‰I”c˜!œb ™d™(“mÑ# AÑ% a§c¡cÑ)ñ*ò*ð +ð *ð �<‰<×Ñ˜AÓ×,Ñ,¨Q¯S©SÓ1´R¸±TÒ9Ð9Ð9äÓ!�J€A€qˆ!ˆQÜ�5‹>�D€A€qÜˆS�!�Q Q¨¯©¡W¸!¸a¿c¹c¹'ØŸc™c A§C¡C™i¨!¯#©#™oØ Ÿs™s Q§S¡S™y¨1¯3©3™õ0ð �S‰S�1—3‘3‰Y˜Ÿ™‰_×+Ñ+¨A¯C©C°!·#±#©I¸¿¹©OÓ<€EÜ�eÓ Ò!Ð!Ð!Ø�S‰S�—‘‰W�Q—S‘S‰[×!Ñ! !Ó$×-Ñ-Ó/Ø”�A“‰Y�q‰[œ1˜Q›4 ™6Ñ! 1§3¡3Ñ&¬!¨A«$¨q©&°1´S¸¼RÀ¹T¹³]±?À1Ñ3DÑ*DÀaÇcÁcÑ)IÑIØŒs�1”r˜!‘t‘8‹}‰_˜QÑ¤ 1£ a¡Ñ'¨¯©Ñ,ñ-ò-ð .ð -ô �A—E‘E˜!“HÓ¤Ü	
ˆ1‹ˆa‰�!”C˜“F‘(˜1‘*Ñ	˜b¤ Q¬¨A©¡X£Ñ.¨qÑ0´1°Q³4¸±6Ñ9Ø	
Œ3ˆq”2�a‘4‰x‹=‰˜Ñ	œQ˜q›T !™VÑ	#ð	%à	
Œ3ˆq”2�a‘4‰x‹=‰˜Ñ	œQ˜q›T !™VÑ	# Q¤s¨1£v¡X¨a¡Z´!°A³$°q±&Ñ%8Ø	
Œ3ˆq”2�a‘4‰x‹=‰˜Ñ	œQ˜q›T !™VÑ	#ð	%à	ŒC�”B�q‘D‘‹MÑ	˜!Ñ	œa ›d 1™fÑ	$ a¬¨A´°1±©H«¡o°aÑ&7¼!¸A»$¸q¹&Ñ&@Ø	
Œ3ˆq‹6‰�!‰”a˜“d˜1‘fÑ	ð	ð)ó " ò  ð  ð  ð �L‰L˜‹O˜q §¡™u q¨¯©¡u™}¨q°·±©uÑ4´d¸1³gÑ=Ñ=×GÑGÔIÐIÐIØ�<‰<˜‹?×"Ñ" 1Ó%×.Ñ.Ó0°Q°q·s±s±U¸Q¸q¿s¹s¹U±]Ø56°q·s±s±Uñ6;Ü<@À»Gñ5Dò Dð Dð Dà�<‰<˜‹?×$Ñ$Ó&¬$¨q°!©t«*Ò4Ð4Ð4Ø�L‰L˜‹O×)Ñ)¨!¯#©#¨a¯c©c©'°!·#±#©+Ó6€EØ�>‰>˜1˜a˜&Ó!×*Ñ*Ó,°Ò1Ð1Ð1Ø�L‰L×Ñ˜QÓ×(Ñ(Ó*Ü�‹G�A‰I�a‰K˜‰M˜!Ÿ#™#Ñ¤ Q£¨¡	¨!¡¨A¡¨a¯c©cÑ 1Ñ1ò2ð 3ð 2à�<‰<× Ñ  §¡Ó.°!°A·C±C±%¸!¸A¿C¹C¹%±-Ò?Ð?Ð?Ø�L‰L×!Ñ! !§,¡,Ó/×7Ñ7¸Ó:×CÑCÓEØ��A”c˜!“f‘H˜q‘LÑ! !Ñ#Ñ# Q§S¡SÑ(¨1¨a´°A¼¸1¹±H³©oØñ/ñ ,Øñ,Ø—‘ñ,ñ Ø˜!œC ¤B q¡D¡›M™/¨AÑ-Ñ.¨qÑ0°·±Ñ4ñ5ò5ð 6ð 5ð �L‰L×Ñ˜QÓ×'Ñ'¨Ó*×3Ñ3Ó5Ø�‰E�!‰G”C˜“F‰N˜1Ñ˜QŸS™SÑ  1 Q¡3 q¡5¬¨Q´°A±©X«Ñ#6°qÑ#8¸¿¹Ñ#<Ñ<Øˆa‰C�‰E”#�aœ"˜Q™$‘h“-Ñ Ñ! !§#¡#Ñ%ñ&ò&ð 'ð &ð �<‰<×Ñ˜AÓ×"Ñ" 1§3¡3¨¯©¡9¨q¯s©s¡?Ó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Ó  € ÜÓ!�J€A€qˆ!ˆQÜ×(Ñ(¨¨Q¯S©S¨D°!·#±#Ó6€AØ�3‰3�!—#‘#Š:Ðˆ:Ü×(Ñ(¨¨Q¯S©S¨D°!·#±#Ó6€AØ�3‰3�!—#‘#Š:Ðˆ:Ü×(Ñ(¨¨Q¯S©S¨D°!·#±#Ó6€AØ�3‰3�!—#‘#Š:Ðˆ:Ü×(Ñ(¨¨Q¯S©S¨D°1·3±3©J¸¿¹¸a¿c¹c¹	ÓB€AØ�3‰3�!—#‘#Š:Ðˆ:Ü×(Ñ(¨¨Q¯S©S¨D°1·3±3©J¸¿¹¸a¿c¹c¹	ÓB€AØ�3‰3�!—#‘#Š:Ðˆ:Ü×(Ñ(¨¨Q¯S©S¨D°1·3±3©J¸¿¹¸a¿c¹c¹	ÓB€AØ�3‰3�!—#‘#Š:Ðˆ:Ü×(Ñ(¨¨Q¯S©S¨D°1·3±3©J¸¿¹Ñ,<¸a¿c¹cÀAÇCÁC¹iÈ!Ï#É#¹oÓN€AØ�3‰3�—‘‰9˜Ÿ™˜aŸc™c™	Ò!Ð!Ñ!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  û€ Ü˜%Ó �D€A€qä+¨DÓ1Ñ€A€qˆ!ˆQ��dÜ��a˜˜A˜q°DÈ4ÔP€AØ�<‰<˜4Ÿ6™6Ò!Ð!Ð!ä�Qœ˜A›¤ A£Ð'Øœ˜Q›˜¤ Q£Ð(Úðó  €Eô ,¨DÓ1Ñ€A€qˆ!ˆQ��dÜˆS�!�Q˜˜1°ÀtØŸ™õà�5‰5�‹8�uÒÐÐÜ+¨DÓ1Ñ€A€qˆ!ˆQ��dÜˆS�!�Q˜˜1°ÀtØŸ™˜õ!à�5‰5�‹8�uÒÐÐä+¨DÓ1Ñ€A€qˆ!ˆ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   ø€ ”8˜C  A°A·C±C·M±MÀ!Ó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°ÀÀAÇCÁCÁÔ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™  ü€ äÓ!�J€A€qˆ!ˆQÜ�S˜!˜Q¨Q¯S©S°1·3±3©YÔ7€EØ×Ñ×"Ñ" kÒ1Ð1Ð1Ø×Ñ×&Ñ& q§|¡|Ó4¸¿¹¸a¿c¹c¹	ÒAÐAÐAØ×Ñ §¡Ò,Ð,Ð,äÓ!�J€A€qˆ!ˆQØ—<‘<×)Ñ)¨#¨q¯s©s°Q·S±S©yÓ9€LÜ�S˜!˜Q¨\Ø!"§¡ô/€Eà×Ñ Ò-Ð-Ð-Ø×Ñ §¡Ò,Ð,Ð,äÓ!�J€A€qˆ!ˆQÜ
Œ9ÝEôGô 	�sÓ€AÜÓ!�J€A€qˆ!ˆQÜ
Œ:ÖHÔIÜÓ!�J€A€qˆ!ˆQØ—,‘,×(Ñ(¨¨a°!·#±#©gÓ6€KÜ
Œ:Õ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   ø€ ”8˜C  AÐ9JÔK€ r2   c                  ó"   •— t        d‰‰ ‰¬«      S r»   r·   )r&   r#   ru   s   €€€r0   r:   z)test_locate_joint_frame.<locals>.<lambda>®  s   ø€ ”8˜C  AÐ8HÔ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  û€ äÓ!�J€A€qˆ!ˆQÜ& {Ó3ÐØ×!Ñ! ! Q§S¡S¨!Ô,Ü�S˜!˜QÐ2CÔD€EØ×"Ñ"Ð&7Ò7Ð7Ð7Ø×"Ñ"×-Ñ-¨aÓ0°AÒ5Ð5Ð5Ø×!Ñ! QÒ&Ð&Ð&ä�sÓ€AÜÓ!�J€A€qˆ!ˆQÜ& {Ó3ÐØ×!Ñ! ! Q§S¡S¨!Ô,Ü
Œ:ÝKôMô  Ó!�J€A€qˆ!ˆQÜ% kÓ2ÐØ× Ñ   A§C¡C¨Ô+Ü
Œ:Ý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¿  € ÜÓ!�J€A€qˆ!ˆQÜ˜*Ó%�D€A€qÜ�s˜A˜qÓ!€AØ�6‰6�SŠ=Ðˆ=Ø�8‰8�qŠ=Ðˆ=Ø�7‰7�aŠ<Ðˆ<Ø�=‰=œF A 3›KÒ'Ð'Ð'Ø�8‰8”v˜q˜c“{Ò"Ð"Ð"Ø�6‰6”V˜Q §¡¬£™]˜OÓ,Ò,Ð,Ð,Ø�<‰<˜1Ÿ7™7Ÿ9™9Ò$Ð$Ð$Ø�=‰=×!Ñ! !§,¡,Ó/´6¸!³9Ò<Ð<Ð<Ø�>‰>×"Ñ" 1§<¡<Ó0´F¸1³IÒ=Ð=Ð=Ø�>‰>×"Ñ" 1§=¡=Ó1°q°S¸1¿7¹7¿9¹9±_ÒDÐDÐDØ�<‰<× Ñ  §¡Ó.°A°#¸¿¹¿	¹	±/ÒAÐAÐAØ�<‰<×Ñ˜AŸG™GÓ$¨¨A¯G©G¯I©I©Ò5Ð5Ð5Ø�7‰7×Ñ˜aŸg™gÓ&¨!Ò+Ð+Ð+Ø�7‰7×Ñ˜aŸg™gÓ&¨!Ò+Ð+Ð+Øˆ1�9‰9‹;ÐBÒBÐBÐBäÓ!�J€A€qˆ!ˆQÜ�5‹>�D€A€qÜ˜'Ó"€DØ×Ñ�Q—W‘W˜aŸc™c¤2¨¡6Ô*Ü�s˜A˜q¨q°1·7±7·9±9©}Ø#$ q§w¡w§y¡y¡=¸Q¿W¹W¿Y¹YØ)-ô	/€Að �<‰<˜1Ÿ7™7Ÿ9™9Ò$Ð$Ð$Ø�=‰=×!Ñ! !§,¡,Ó/°1°q·w±w·y±y±=Ò@Ð@Ð@Ø�>‰>×"Ñ" 1§<¡<Ó0°A¸¿¹¿	¹	±MÒAÐAÐAØ�>‰>×"Ñ" 1§=¡=Ó1°q°S¸1¿7¹7¿9¹9±_ÒDÐDÐDØ�<‰<× Ñ  §¡Ó.°A°#¸¿¹±)¸aÀ!Ç#Á#¹gÑ2EÈÈAÏCÉCÉÑ2OÒOÐOÐOØ�<‰<×Ñ˜AŸG™GÓ$¨¨A¯G©G¯I©I©Ò5Ð5Ð5Ø�<‰<×Ñ˜DÓ!¤V¨A£YÒ.Ð.Ð.Ø�7‰7×Ñ˜aŸg™gÓ&¨!Ò+Ð+Ð+Ø�7‰7×Ñ˜aŸg™gÓ&¨!Ò+Ð+Ð+äÓ!�J€A€qˆ!ˆQÜ˜'Ó"€DØ×Ñ�Q—W‘W˜aŸc™c¤2¨¡6Ô*Ü�s˜A˜q¨q°1·7±7·9±9©}Ø#$ q§w¡w§y¡y¡=¸Q¿W¹W¿Y¹YØ)-ô	/€Að �<‰<˜1Ÿ7™7Ÿ9™9Ò$Ð$Ð$Ø�=‰=×!Ñ! !§,¡,Ó/°1°q·w±w·y±y±=Ò@Ð@Ð@Ø�>‰>×"Ñ" 1§<¡<Ó0°A¸¿¹¿	¹	±MÒAÐAÐAØ�>‰>×"Ñ" 1§=¡=Ó1°q°S¸1¿7¹7¿9¹9±_ÒDÐDÐDØ�<‰<× Ñ  §¡Ó.°A°2¸±6¸1¿7¹7¿9¹9Ñ2DÀqÈÏÉÏÉÁ{Ñ2RÒRÐRÐRØ�<‰<×Ñ˜AŸG™GÓ$¨¨A¯G©G¯I©I©Ò5Ð5Ð5Ø�7‰7×Ñ˜aŸg™gÓ&¨!Ò+Ð+Ð+Ø�7‰7×Ñ˜aŸg™gÓ&¨!Ò+Ð+Ñ+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Æ  € Ü˜*Ó%�D€A€qäÓ!�J€A€qˆ!ˆQÜ�3˜˜1°·±¨tÕ4à�S‰SˆD×Ñ §¡Ó$¨Ò)Ð)Ð)Ø�C‰C�K‰K˜‹Nˆ?˜aŸc™cÒ!Ð!Ð!Ø�5‰5�‹8”všzª:²yÐAÓBÒBÐBÐBØ�<‰<× Ñ  §¡Ó.°!°a·c±c±'Ò9Ð9Ð9Ø�<‰<× Ñ  §¡Ó.×6Ñ6°qÓ9×BÑBÓDÈÈÈQÏSÉSÉÒPÐPÐPØ�<‰<×Ñ˜AÓ ! a§c¡c¡'Ò)Ð)Ð)Ø�<‰<×Ñ˜AÓ×&Ñ& qÓ)¨a¨R°!·#±#©XÒ5Ð5Ð5Ø�<‰<˜‹?˜aÒÐÐØ�<‰<˜‹?˜aÒÐÐô  Ó!�J€A€qˆ!ˆQÜ�3˜˜1¨q¯s©sÀÇÁÕDà�3‰3×Ñ˜QŸS™SÓ! QÒ&Ð&Ð&Ø�3‰3�;‰;�q‹>˜QŸS™SÒ Ð Ð Ø�5‰5�‹8”všzª9²iÐ@ÓAÒAÐAÐAØ�<‰<×Ñ˜AÓ ! a§c¡c¡'Ò)Ð)Ð)Ø�<‰<×Ñ˜AÓ×&Ñ& qÓ)¨Q°·±©WÒ4Ð4Ð4Ø�<‰<× Ñ  §¡Ó.°!°A·C±C±%¸!¿#¹#±+Ò=Ð=Ð=Ø�<‰<× Ñ  §¡Ó.×6Ñ6°qÓ9×BÑBÓDÈÈ!Ï#É#ÉÐPQ×PSÑPSÉÒSÐSÐSØ�<‰<˜‹?˜aÒÐÐØ�<‰<˜‹?˜aÒÐÐô  Ó!�J€A€qˆ!ˆQÜ�3˜˜1°·±À!Ç#Á#ÕFà�3‰3×Ñ˜QŸS™SÓ! QÒ&Ð&Ð&Ø�3‰3�;‰;�q‹>˜aŸc™cÒ!Ð!Ð!Ø�5‰5�‹8”všyª*²iÐ@ÓAÒAÐAÐAØ�<‰<×Ñ˜AÓ ! a§c¡c¡'Ò)Ð)Ð)Ø�<‰<×Ñ˜AÓ×&Ñ& qÓ)¨Q°·±©WÒ4Ð4Ð4Ø�<‰<× Ñ  §¡Ó.°!·#±#¸¸!¿#¹#¹±+Ò=Ð=Ð=Ø�<‰<˜‹?˜aÒÐÐØ�<‰<˜‹?˜aÒÐÐô  Ó!�J€A€qˆ!ˆQÜ�3˜˜1¨1¯3©3¸A¿C¹CØ$%§C¡C¨!¯#©#¡Iõ/à�3‰3×Ñ˜QŸS™S 1§3¡3™YÓ'¨1Ò,Ð,Ð,Ø�C‰C�!—#‘#‰I×Ñ˜qÓ!¤T¨!£W¨Q¯S©S¡[Ò0Ð0Ð0Ø�5‰5�‹8”v¤ Q£¨¡	¬D°«G¨8°A©:°qÐ9¼DÀ»GÀA¹IÄtÈAÃwÈqÁyÐRSÐ;TÒV_Ð`ÓaÒaÐaÐaØ�<‰<× Ñ  §¡Ó.°1°q±5¸!¿#¹#±+ÀÇÁÑ2CÒCÐCÐCØ�<‰<× Ñ  §¡Ó.×6Ñ6°qÓ9¸aÄ$ÀqÃ'È!Á)¹mÈaÑ>OÐQR×QTÑQTÑ=TÔW[Ð\]ÓW^Ð_`ÑW`Ðab×adÑadÑWdÑ=dÒdÐdÐdØ�<‰<×Ñ˜AÓ×&Ñ& qÓ)¨Q°!·#±#¸¿¹±)©_¼TÀ!»WÑ-DÒDÐDÐDØ�<‰<×Ñ˜AÓ ! A§C¡C¡%Ò'Ð'Ð'Ø�<‰<˜‹?˜aÒÐÐØ�<‰<˜‹?˜aÒÐÐäÓ!�J€A€qˆ!ˆQÜ�3˜˜1¨1¯3©3¸A¿C¹CØ$%§C¡C¨!¯#©#¡I°·±¡Oõ5à�3‰3×Ñ˜QŸS™S 1§3¡3™Y¨¯©™_Ó-×6Ñ6Ó8¸AÒ=Ð=Ð=Ø�S‰S�1—3‘3‰Y˜Ÿ™‰_×%Ñ% aÓ(¬4°«7°1·3±3©;Ñ6×@Ñ@ÓBÀaÒGÐGÐGÜ�A—E‘E˜!“HÓ¤¬$¨q«'°!©)´d¸1³g°X¸a±ZÄÀaÃÈÁÐ)KÜ26°q³'¸!±)¼TÀ!»WÀQ¹YÌÈ1ËÈaÉÑ=OÔQRÐSTÓQUÐVWÑQWÔZ^Ð_`ÓZaÐbcÑZcÑQcÐ1dÜ37¸³7°(¸1±*¼aÀ»dÀ1¹fÄtÈAÃwÈqÁyÑ>PÔRVÐWXÓRYÐZ[ÑR[Ô^_Ð`aÓ^bÐcdÑ^dÑRdÐ1eð)gó "hò hð hð hð �<‰<× Ñ  §¡Ó.°1°q±5¸!¿#¹#±+ÀÇÁÑ2CÒCÐCÐCØ�L‰L×!Ñ! !§,¡,Ó/×7Ñ7¸Ó:Ø
Œd�1‹g�a‰i‰-˜!Ñ
˜QŸS™SÑ	 ¤4¨£7¨1¡9¨Q¯S©S¡=Ñ	0´4¸³7¸1±9¸Q¿S¹S±=Ñ	@ñBßCKÁ8Ã:ÐQRòSð Sð Sà�<‰<×Ñ˜AÓ ! A§C¡C¡%Ò'Ð'Ð'Ø�L‰L×Ñ˜QÓ×'Ñ'¨Ó*ÜˆQ‹�‰	�!‰�A—C‘C‰œ$˜q›' !™) A™+ a§c¡c™/Ñ)¬D°«G°A©I°a©K¸¿¹©OÑ;ñ=ß>F¹h»jðIð Ið Ià�<‰<˜‹?˜aÒÐÐØ�<‰<˜‹?˜aÒÐÐäÓ!�J€A€qˆ!ˆQÜ�5‹>�D€A€qÜ�3˜˜1¨1¨Q¯S©S©5¸aÀÇÁ¹eØ$%§C¡C¨!¯#©#¡I°·±¡OØ%&§S¡S¨1¯3©3¡Y°·±¡_õ6ð �C‰C�—‘‰G�A—C‘C‰K×&Ñ& q§s¡s¨1¯3©3¡w¨q¯s©s¡{Ó3×<Ñ<Ó>À!ÒCÐCÐCØ�S‰S�—‘‰W�Q—S‘S‰[×!Ñ! !Ó$¨¯©¨a¯c©c©	°A·C±C©Ñ8×BÑBÓDÈÒIÐIÐIÜ�A—E‘E˜!“HÓ¤¬1¨Q«4¨%°©'´A°a³D°5¸±7¼A¸a»DÀ¹FÐ)CÜ23°A³$°q±&¼!¸A»$¸q¹&Ä!ÀAÃ$ÀqÁ&Ð1IÜ34°Q³4°%¸±'¼1¸Q»4À¹6Ä1ÀQÃ4ÈÁ6Ð1Jð)Ló "Mò Mð Mð Mð �L‰L×!Ñ! !§,¡,Ó/Ø	
ŒT�!‹W�Q‰Y�q‰[‰˜!Ÿ#™#Ñ¤ Q£¨¡	¨!¡¨A¯C©C¡Ñ/´$°q³'¸!±)¸A±+¸a¿c¹c±/ÑAÀAÀaÇcÁcÁEÑIñKç‰g�a‹jŸ™› qò)ð )ð )ð �L‰L×!Ñ! !§,¡,Ó/×7Ñ7¸Ó:Ø	
ˆQˆq‰S‰”4˜“7˜1‘9˜Q‘;Ñ	 §¡Ñ# q¨¡s¨1¡u¬t°A«w°q©y¸©{Ñ':¸A¿C¹CÑ&?Ñ?Ø	ˆA‰ˆa‰”$�q“'˜!‘)˜A‘+Ñ	˜qŸs™sÑ"ñ	#ñ$ç%-¡X£Z°1ò5ð 5ð 5ð �L‰L×Ñ˜QÓ×'Ñ'¨Ó*ÜˆQ‹�‰	�!‰�A—C‘C‰œ$˜q›' !™) A™+ a§c¡c™/Ñ)¬D°«G°A©I°a©K¸¿¹©OÑ;ñ=ß>F¹h»jÈAòNð Nð Nà�L‰L×Ñ˜QÓ×'Ñ'¨Ó*Ü�!‹W�Q‰Y�q‰[˜Ÿ™‰_œt A›w q™y¨™{¨1¯3©3™Ñ.´°a³¸±¸1±¸Q¿S¹S±Ñ@ñBßCKÁ8Ã:Ð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½  € ÜÓ!�J€A€qˆ!ˆQÜ%3Ð4DÓ%EÑ"€FˆF�F˜FÜ	˜#˜q !Ó	$€BØ�7‰7�cŠ>Ðˆ>Ø�9‰9˜Š>Ðˆ>Ø�8‰8�qŠ=Ðˆ=Ø�>‰>œV V¨VÐ$4Ó5Ò5Ð5Ð5Ø�9‰9œ ¨Ð/Ó0Ò0Ð0Ð0Ø×!Ñ! VÒ+Ð+Ð+Ø×$Ñ$¨Ò.Ð.Ð.Ø×Ñ Ò&Ð&Ð&Ø×Ñ 6Ò)Ð)Ð)Ø�7‰7”f˜f v§{¡{´1£~Ñ5°vÀÇÁÌAÃÑ7NÐOÓPÒPÐPÐPØ�=‰=˜AŸC™CÒÐÐØ�>‰>×"Ñ" 1§<¡<Ó0´F¸1³IÒ=Ð=Ð=Ø�?‰?×#Ñ# A§L¡LÓ1´V¸A³YÒ>Ð>Ð>Ø�?‰?×#Ñ# B§O¡OÓ4¸¸À!Ç#Á#¹ÒEÐEÐEØ�<‰<× Ñ  §¡Ó.°&¸1¿3¹3±,Ò>Ð>Ð>Ø�>‰>×Ñ˜aÓ  F¨Q¯S©S¡LÒ0Ð0Ð0Ø�<‰<˜‹?˜f q§s¡s™lÒ*Ð*Ð*Ø×Ñ 1Ò$Ð$Ð$Ø×Ñ !Ò#Ð#Ð#Ø�:‰:‹<ÐEÒEÐEÐEä# LÓ1�N€BˆˆB�Ü�6‹?�D€A€qÜ+¨DÓ1Ñ€A€qˆ!ˆQ��dÜ	˜#˜q !¸ÈBØ,.¸QÀÇÁ¹WØ&'¨!¯#©#¡gÀØ+/¸AÀÇÁ¹Gô
E€Bð �>‰>œV R¨ LÓ1Ò1Ð1Ð1Ø�9‰9œ  B˜xÓ(Ò(Ð(Ð(Ø×!Ñ! RÒ'Ð'Ð'Ø×$Ñ$¨Ò.Ð.Ð.Ø×Ñ Ò"Ð"Ð"Ø×Ñ 2Ò%Ð%Ð%Ø�7‰7”f˜b 2§7¡7¬1£:™o¨r°F·K±KÄ³NÑ/BÐCÓDÒDÐDÐDØ�=‰=˜A §¡™GÒ#Ð#Ð#Ø�>‰>×"Ñ" 1§<¡<Ó0°A¸¿¹±GÒ;Ð;Ð;Ø�?‰?×#Ñ# A§L¡LÓ1°Q¸¿¹±WÒ<Ð<Ð<Ø�?‰?×#Ñ# B§O¡OÓ4¸¸À!Ç#Á#¹ÒEÐEÐEØ�<‰<× Ñ Ø	�‰óØ˜QŸS™S™ 6¨A¯C©C¡<Ñ/°!°a·c±c±'Ñ9ò:ð :ð :à�<‰<×Ñ˜AÓ " q§s¡s¡(¨R°!©V°a·c±c©\Ñ"9Ò9Ð9Ð9Ø�<‰<˜‹?˜b 1§3¡3™hÒ&Ð&Ñ&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³  € ÜÓ!�J€A€qˆ!ˆQÜ+¨HÓ5Ñ€FˆF�FÜ+¨HÓ5Ñ€FˆF�FÜ	�S˜!˜QÓ	€BØ�7‰7�cŠ>Ðˆ>Ø�9‰9˜Š>Ðˆ>Ø�8‰8�qŠ=Ðˆ=Ø�>‰>œV V¨V°VÐ$<Ó=Ò=Ð=Ð=Ø�9‰9œ ¨°Ð7Ó8Ò8Ð8Ð8Ø×!Ñ! VÒ+Ð+Ð+Ø× Ñ ¤F¨F°FÐ+;Ó$<Ò<Ð<Ð<Ø×Ñ Ò&Ð&Ð&Ø×Ñœv v¨vÐ&6Ó7Ò7Ð7Ð7Ø�7‰7”f˜f v§{¡{´1£~Ñ5°vÀÇÁÌAÃÑ7NØ$ v§{¡{´1£~Ñ5ð7ó 8ò 8ð 8ð 8à×Ñ˜qŸs™sÒ"Ð"Ð"Ø×Ñ §¡ a§c¡c 
Ò*Ð*Ð*Ø�>‰>×"Ñ" 1§<¡<Ó0´F¸1³IÒ=Ð=Ð=Ø�?‰?×#Ñ# A§L¡LÓ1´V¸A³YÒ>Ð>Ð>Ø�Q—S‘S‰L˜6 A§C¡C™<Ñ'€EØ�?‰?×#Ñ# B§N¡NÓ3¸°vÒ=Ð=Ð=Ø�<‰<× Ñ  §¡Ó.°%Ò7Ð7Ð7Ø�>‰>×Ñ˜aÓ  F¨Q¯S©S¡L°6¸A¿C¹C±<Ñ$?Ò?Ð?Ð?Ø�<‰<˜‹?˜f q§s¡s™lÒ*Ð*Ð*Ø×Ñ 1Ò$Ð$Ð$Ø×Ñ !Ò#Ð#Ð#Ø�:‰:‹<Ð@Ò@Ð@Ð@ä+¨LÓ9Ñ€BˆˆB��B˜Ü�6‹?�D€A€qÜ+¨DÓ1Ñ€A€qˆ!ˆQ��dÜ	�S˜!˜Q°BØ)+¨R¨ÀÀRÀØ"# a§c¡c¡'°q¸1¿3¹3±wØ'+¸dô
D€Bð �>‰>œV R¨¨R LÓ1Ò1Ð1Ð1Ø�9‰9œ ¨¨BÐ/Ó0Ò0Ð0Ð0Ø×!Ñ! RÒ'Ð'Ð'Ø× Ñ ¤F¨B°¨8Ó$4Ò4Ð4Ð4Ø×Ñ Ò&Ð&Ð&Ø×Ñœv r¨2 hÓ/Ò/Ð/Ð/Ø�7‰7”f˜f r§w¡w¬q£zÑ1°2¸¿¹Ä»
±?Ø  2§7¡7¬1£:™oð/ó 0ò 0ð 0ð 0à×Ñ˜tŸv™vÒ%Ð%Ð%Ø×Ñ §¡¨¯©Ð 0Ò0Ð0Ð0Ø�>‰>×"Ñ" 1§<¡<Ó0°A¸¿¹±GÒ;Ð;Ð;Ø�?‰?×#Ñ# A§L¡LÓ1°Q¸¿¹±WÒ<Ð<Ð<Ø�?‰?×#Ñ# B§N¡NÓ3°r¸A¿C¹C±xÀ"ÀqÇsÁsÁ(Ñ7JÒJÐJÐJØ�<‰<× Ñ Ø	�‰óØ˜QŸS™S™ 2¨¯©¡8Ñ+¨b°1·3±3©hÑ6¸¸Q¿S¹S¹Ñ@òAð Að Aà�<‰<×Ñ˜AÓ 2 #¨¯©¡)¨b°1·3±3©hÑ"6¸À!¹ÀaÇcÁcÑ9IÑ"IÒIÐIÐIØ�<‰<˜‹?˜f q§s¡s™lÒ*Ð*Ñ*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‹_�F€BˆÜÓ!�J€A€qˆ!ˆQÜ�C˜˜A˜r B¨ 8¨R°"°b°Ø!# a§c¡c¡Ø!#  a§c¡c¡	Ø&'§c¡c¨A¯C©C´$°q³'©MÑ&9Ø%&§S¡S¨1¯3©3´°a³©=Ñ%8ô		:€Að
 �?‰?×"Ñ" 1Ó%¨!¯#©#°·±´d¸1³g±Ñ*=×)HÑ)HÓ)JÒJÐJÐJØ�?‰?×"Ñ" 1Ó%¨!¯#©#°·±´d¸1³g±Ñ*=×)HÑ)HÓ)JÒJÐJÐJØ�?‰?×(Ñ(¨¯©Ó-´°a±Ò7Ð7Ð7Ø�5‰5�‹8×Ñ˜b ! R¨¨B°Ð2Ó3´s¸1³vÒ=Ð=Ð=ÜÜ	ˆR‹”4˜“7�(œS ›WÑ$ qÑ(¬#¨b«'°A©+Ð6Ü	ˆa‹”3�r“7Ñ	˜QÑ	 ¤C¨£G¡¨a¡°%Ñ 7Ü	ˆa‹�Aœ˜B›‘KÑ	  1Ñ	$ð	&ä
ˆb‹'ˆ�A‰”t˜A“w !¤c¨"£g¡+Ñ.°Ñ2´C¸³G¸a±KÀ%Ñ4GÐHð	Jó K€Eô �A—E‘E˜!“H˜uÑ$Ó%¬¨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  € ÜÓ!�J€A€qˆ!ˆQÜ+Ð,<Ó=Ñ€BˆˆB��B˜Ü�s˜A˜qÓ!€AØ�6‰6�SŠ=Ðˆ=Ø�8‰8�qŠ=Ðˆ=Ø�7‰7�aŠ<Ðˆ<Ø�=‰=œF B¨¨B <Ó0Ò0Ð0Ð0Ø�8‰8”v˜r 2 r˜lÓ+Ò+Ð+Ð+Ø�6‰6”V˜R "§'¡'¬!£*™_¨b°2·7±7¼1³:©o¸rÀBÇGÁGÌAÃJ¹ÐOÓPÒPÐPÐPØ�=‰=×!Ñ! !§,¡,Ó/´6¸!³9Ò<Ð<Ð<Ø�>‰>×"Ñ" 1§<¡<Ó0´F¸1³IÒ=Ð=Ð=Ø�>‰>×"Ñ" 1§=¡=Ó1´V¸A³YÒ>Ð>Ð>Ø�<‰<× Ñ  §¡Ó.´&¸³)Ò;Ð;Ð;Ø�<‰<×Ñ˜AÓ¤&¨£)Ò+Ð+Ð+Ø�<‰<˜‹? ˜s¤S¨£W™}¬s°2«wÑ6¸¼cÀ"»g¹ÑEÈÏÉÑLØ”�R“‰Lœ3˜r›7Ñ" R¬#¨b«'¡\Ñ1°Q·S±SñP9ñ 9à�3œ˜R›‘= 2Ñ%¨¯©ñ<-ñ-ò -ð -ð -ð �<‰<˜‹?˜r¤C¨£G™|¬c°"«gÑ5¸¼SÀ»W¹ÑDÈÏÉÑKØˆC”#�b“'‰MœC ›GÑ# b¬3¨r«7¡lÑ2°a·c±cñO:ñ :àœ˜B›‘< "Ñ$¨¯©ñ=,ñ,ò ,ð ,ð ,ð ˆ1�9‰9‹;Ð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>  € ô   Ó'�J€BˆˆBÜ ¨Ó*�J€BˆˆBÜÓ!�J€A€qˆ!ˆQÜ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<ÔP€AØ�=‰=œF B¨¨B <Ó0Ò0Ð0Ð0Ø�8‰8”v˜r 2 r˜lÓ+Ò+Ð+Ð+Ø�6‰6”VšIÓ&Ò&Ð&Ð&Ø�<‰<˜‹? ˜s¤S¨£W™}¬s°2«wÑ6¸¼cÀ"»g¹ÑEÈÏÉÑLØ
ŒS�‹W‰”s˜2“wÑ ¤c¨"£g¡Ñ-°·±ñP5ñ 5àˆs”S˜“W‰}˜rÑ! Q§S¡Sñ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¨!¬V°R¸°HÓ-=¼vÀrÀd»|Ó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�”6˜2˜r 2 rÐ*Ó+¬V°R¸¸R°LÓ-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�”6˜2˜r 2˜,Ó'¬°°R¸¸RÐ0@Ó)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  ÿù€ Ü#1Ð2BÓ#CÑ €Cˆˆc�3˜˜SÜ%3°LÓ%AÑ"€BˆˆB��B˜˜B äÓ!�J€A€qˆ!ˆQÜ�s˜A˜q 2 r¨2 ,°°R¸°Ó=€AØ�=‰=œF B¨¨B <Ó0Ò0Ð0Ð0Ø�8‰8”v˜r 2 r˜lÓ+Ò+Ð+Ð+äÓ!�J€A€qˆ!ˆQÜ�s˜A˜q¤&¨"¨b°"¨Ó"6Ü˜r 2 r˜lÓ+ó	-€Aà�=‰=œF B¨¨B <Ó0Ò0Ð0Ð0Ø�8‰8”v˜r 2 r˜lÓ+Ò+Ð+Ð+äÓ!�J€A€qˆ!ˆ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˜“G�8œc "›gÑ%¤s¨2£w´°R³Ñ'8Ð9Ü
ˆb‹'ˆ”C˜“GÑ	œS ›W¤s¨2£wÑ.´°R³Ñ8¼3¸r»7ÄSÈÃWÑ;LÑLÜ
ˆb‹'ˆ”C˜“GÑ	œc "›gÑ	%¬¨B«´#°b³'Ñ(9Ñ	9ð	;ä	ˆR‹”3�r“7Ñ	œS ›W˜H¤s¨2£wÑ.´°R³¼3¸r»7Ñ1BÄSÈÃWÑ1LÑLÜ
ˆb‹'ˆ”C˜“GÑ	œc "›g¬¨B«Ñ/´#°b³'Ñ9Ñ	9ð	;ð	<ó =€Eô �b�Sœ3˜r›7‘] RÑ'Ð(Ø�Sœ3˜r›7‘]¤S¨£WÑ,¨r´C¸³G©|Ñ;Ð<Øœ#˜b›'‘\¤C¨£GÑ+¨b´3°r³7©lÑ:Ð;ð=ó >€Eô Ü
ˆq‹'ˆ�Rœ#˜b¤2¨¡6™kÓ*Ñ*¬S°«WÑ4°r¼CÀÄRÈ!ÁVÁÓ<LÑ7LÑLÑ	MÐNØ
ˆŒs�2‹w‰˜Ñ	Ð ˜s¤S¨£W™}¨rÑ1Ð2ð4ó 5€Fô ,¨DÓ1Ñ€A€qˆ!ˆQ��dÜ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<Ø$%§C¡C¨!¯#©#¡I¸A¿C¹C¸4À!Ç#Á#¹:Ø)-ÀØ &°#ô	7€Að �;‰;×ÑÓ &Ò(Ð(Ð(Ø�<‰<˜3ÒÐÐÜ�A—E‘E˜!“H˜uÑ$Ó%¬¨q«Ò1Ð1Ð1Ü�A—L‘L “O×-Ñ-¨aÓ0°5Ñ8Ó9¼UÀ1Àa»[ÒHÐHÐHÜ�A—L‘L×$Ñ$ QÓ'×1Ñ1°!Ó4Ó5¸Ò?Ð?Ð?ä+¨DÓ1Ñ€A€qˆ!ˆQ��dÜ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<Ø$%§C¡C¨!¯#©#¡I¸A¿C¹C¸4À!Ç#Á#¹:Ø)-ÀØ &°°R¸°Èô	M€A÷:ð :€Là�;‰;×ÑÓ &Ò(Ð(Ð(Ø�<‰<˜3ÒÐÐÜ�A—E‘E˜!“H™|¨EÓ2Ñ2Ó3´u¸Q³xÒ?Ð?Ð?Ü�A—L‘L “O×-Ñ-¨aÓ0±<ÀÓ3FÑFó Ü!& q¨!£ò-ð -ð -ä�A—L‘L×$Ñ$ QÓ'×1Ñ1°!Ó4Ó5¹ÀfÓ9MÒMÐMÐMä+¨DÓ1Ñ€A€qˆ!ˆQ��dÜ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<Ø$%§C¡C¨!¯#©#¡I¸A¿C¹C¸4À!Ç#Á#¹:Ø)-ÀØ &°%ô	9€Að �;‰;×ÑÓ &Ò(Ð(Ð(Ø�<‰<˜5Ò Ð Ð Ü�A—E‘E˜!“HœvÜ
ˆb‹'ˆ”C˜“GÑ	œS ›W¤s¨2£wÑ.´°R³Ñ8¼3¸r»7ÄSÈÃWÑ;LÑLÜ	ˆR‹”3�r“7Ñ	œS ›WÑ	$¤s¨2£w´°R³Ñ'8Ñ	8ð	:ä
ˆb‹'ˆ”C˜“G�8œc "›gÑ%¬¨B« x´#°b³'Ñ'9Ð:Ü	ˆR‹”3�r“7Ñ	œS ›W˜H¤s¨2£wÑ.´°R³¼3¸r»7Ñ1BÄSÈÃWÑ1LÑLÜ	ˆR‹”3�r“7Ñ	œS ›W¤s¨2£wÑ.´°R³Ñ8Ñ	8ð	:ð	';ó  <ñ <ó =ô
 AFÀaÃòIð Ið Iô �A—L‘L “O×-Ñ-¨aÓ0´6Ø	Œc�"‹g‰˜Ñ	Ð˜b¤3 r£7™l¬S°«WÑ4°r¼CÀ»G±|ÑCÐDØ	Œc�"‹g‰œ˜B›Ñ	 "¤s¨2£w¡,Ñ	.Ð/ð;1ó 42ñ 2ó 3ä6;¸A¸q³kòBð Bð Bô �A—L‘L×$Ñ$ QÓ'×1Ñ1°!Ó4Ó5¼Ü
ˆq‹'ˆ�Rœ#˜b¤2¨¡6™kÓ*Ñ*¬S°«WÑ4°r¼CÀÄRÈ!ÁVÁÓ<LÑ7LÑLÑ	MÐNØ	Œc�"‹g‰˜Ñ	Ð˜b¤3 r£7™l¨RÑ/Ð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‹”3�r“7Ñ	œS ›WÑ	$¤s¨2£w´°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Ñ	9¼CÀ»G¸8ÄcÈ"ÃgÑ;Mð	Oä	ˆR‹”3�r“7Ñ	œS ›W˜H¤s¨2£wÑ.´°B³Ð8ð:ó ;€Eô Ø	Œc�"‹g‰˜œS ›W™¤s¨2£wÑ.Ñ	.Ð/Ø	Œc�"‹g‰˜œS ›W™¤s¨2£wÑ.Ñ	.Ð/°"°r¼CÀ»G±|Ñ2CÐ1DðFó G€Eô Ø	ˆb”3�r“7‰lÑ	Ð˜b 2¬¨B«¡<Ñ/Ð0Ü	ˆa‹�R�Cœ#˜b¤2¨¡6™kÓ*Ñ*¨R´#°b¼2À¹6±kÓ2BÑ-BÄSÈÃWÑ-LÑLÑ	MÐNðPó Q€Fô ,¨DÓ1Ñ€A€qˆ!ˆQ��dÜ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<Ø$%§C¡C¨!¯#©#¡I¸A¿C¹C¸4À!Ç#Á#¹:Ø)-ÀØ '°3ô	8€Að �;‰;×ÑÓ 'Ò)Ð)Ð)Ø�<‰<˜3ÒÐÐÜ�A—E‘E˜!“H˜uÑ$Ó%¬¨q«Ò1Ð1Ð1Ü�A—L‘L “O×-Ñ-¨aÓ0Ó1°UÒ:Ð:Ð:Ü�A—L‘L×$Ñ$ QÓ'×1Ñ1°!Ó4Ó5¸Ò?Ð?Ð?÷:ð :€Lä+¨DÓ1Ñ€A€qˆ!ˆQ��dÜ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<Ø$%§C¡C¨!¯#©#¡I¸A¿C¹C¸4À!Ç#Á#¹:Ø)-ÀØ '°"°b¸"°Èô	N€Að �;‰;×ÑÓ 'Ò)Ð)Ð)Ø�<‰<˜3ÒÐÐÜ�A—E‘E˜!“H™|¨EÓ2Ñ2Ó3´u¸Q³xÒ?Ð?Ð?Ü�A—L‘L “O×-Ñ-¨aÓ0Ó1±\À%Ó5HÒHÐHÐHÜ�A—L‘L×$Ñ$ QÓ'×1Ñ1°!Ó4Ó5¹ÀfÓ9MÒMÐMÐMä+¨DÓ1Ñ€A€qˆ!ˆQ��dÜ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<Ø$%§C¡C¨!¯#©#¡I¸A¿C¹C¸4À!Ç#Á#¹:Ø)-ÀØ '°5ô	:€Að �;‰;×ÑÓ 'Ò)Ð)Ð)Ø�<‰<˜5Ò Ð Ð Ü�A—E‘E˜!“HœvÜ
ˆb‹'ˆ”C˜“GÑ	œc "›g˜X¬¨B«Ñ/´#°b³'¼CÀ»GÑ2CÄcÈ"ÃgÑ2MÑMÜ	ˆR‹”3�r“7Ñ	œS ›WÑ	$¤s¨2£w´°R³Ñ'8Ñ	8ð	:ä
ˆb‹'ˆ”C˜“G�8œc "›gÑ%¬¨B« x´#°b³'Ñ'9Ð:Ü	ˆR‹”3�r“7Ñ	œS ›W˜H¤s¨2£wÑ.´°R³¼3¸r»7Ñ1BÄSÈÃWÑ1LÑLÜ
ˆb‹'ˆ”C˜“GÑ	œc "›gÑ	%¬¨B«´#°b³'Ñ(9Ñ	9ð	;ð	'<ó  =ñ =ô >ð >ð >ô �A—L‘L “O×-Ñ-¨aÓ0´6Ø
ˆˆr”C˜“G‰|Ñ	Ð ˜s¤S¨£W™}¨r´C¸³G©|¼cÀ"»gÑ/EÑEÐFØ	Œc�"‹g‰˜œS ›W™¤s¨2£wÑ.Ñ	.Ð/ð;1ó 42ñ 2ó 3ä6;¸A¸q³kòBð Bð Bô �A—L‘L×$Ñ$ QÓ'×1Ñ1°!Ó4´vØ	Œc�"‹g‰˜œS ›W™¤s¨2£wÑ.Ñ	.Ð/Ø	Œc�"‹g‰˜œS ›W™¤s¨2£wÑ.Ñ	.Ð/Ø	ˆb”3�r“7‰lÑ	˜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»  € ÜÓ!�J€A€qˆ!ˆQÜ�#�q˜!Ó€AØ�6‰6�SŠ=Ðˆ=Ø�8‰8�qŠ=Ðˆ=Ø�7‰7�aŠ<Ðˆ<Ø�=‰=œF›HÒ$Ð$Ð$Ø�8‰8”v“xÒÐÐØ�6‰6”V˜A˜q "Ó%×'Ñ'Ò'Ð'Ð'Ø�7‰7�;‰;�q—w‘wÓ¤3 q£6Ò)Ð)Ð)Ø�=‰=×!Ñ! !§,¡,Ó/´6¸!³9Ò<Ð<Ð<Ø�>‰>×"Ñ" 1§<¡<Ó0´F¸1³IÒ=Ð=Ð=Ø�>‰>×"Ñ" 1§=¡=Ó1´V¸A³YÒ>Ð>Ð>Ø�<‰<× Ñ  §¡Ó.´&¸³)Ò;Ð;Ð;Ø�<‰<×Ñ˜AŸG™GÓ$¬¨q«	Ò1Ð1Ð1Ø�7‰7×Ñ˜aŸg™gÓ&¨!Ò+Ð+Ð+Ø�7‰7×Ñ˜aŸg™gÓ&¨!Ò+Ð+Ð+Ø�9‰9‹;Ð=Ò=Ð=Ð=äÓ!�J€A€qˆ!ˆQÜ�5‹>�D€A€qÜ˜'Ó"€DØ×Ñ�Q—W‘W˜aŸc™c¤2¨¡6Ô*Ü�#�q˜!¨!¨a¯g©g¯i©i©-Ø !§'¡'§)¡)™m¸tô	E€Að �=‰=×!Ñ! !§,¡,Ó/°1°q·w±w·y±y±=Ò@Ð@Ð@Ø�>‰>×"Ñ" 1§<¡<Ó0°A¸¿¹¿	¹	±MÒAÐAÐAØ�>‰>×"Ñ" 1§=¡=Ó1´V¸A³YÒ>Ð>Ð>Ø�<‰<× Ñ  §¡Ó.°A°#¸¿¹±)¸aÀ!Ç#Á#¹gÑ2EÒEÐEÐEØ�<‰<×Ñ˜AŸG™GÓ$¬¨q«	Ò1Ð1Ð1Ø�<‰<×Ñ˜DÓ!¤V¨A£YÒ.Ð.Ð.Ø�7‰7×Ñ˜aŸg™gÓ&¨!Ò+Ð+Ð+Ø�7‰7×Ñ˜aŸg™gÓ&¨!Ò+Ð+Ð+Ø�3‰3�!—#‘#Š:Ðˆ:ä	Ó	!ñ Ü�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  € Ü˜*Ó%�D€A€qÜÓ!�J€A€qˆ!ˆQÜ	Ó	!ñ -Ü��a˜¨¯© tÕ,÷-à�S‰SˆD×Ñ §¡Ó$¨Ò)Ð)Ð)Ø�C‰C�K‰K˜‹Nˆ?˜aŸc™cÒ!Ð!Ð!Ø�5‰5�‹8”všzØ !¤C¨£F 7¬S°«V¨GÐ4Ø !¤C¨£F 7¬C°«FÐ3ð5ó 6ò 6ð 6ð 6ð �<‰<˜‹?˜a !§#¡#™gÒ%Ð%Ð%Ø�<‰<˜‹?×$Ñ$Ó&¬$¨q°A©v«,Ò6Ð6Ð6äÓ!�J€A€qˆ!ˆQÜ	Ó	!ñ 9Ü�s˜A˜q¨a¯c©c°A·C±C©iÕ8÷9à�C‰C×Ñ˜qŸs™s Q§S¡S™yÓ)¨QÒ.Ð.Ð.Ø�3‰3�;‰;�q‹>˜aŸc™c A§C¡C™i¬4°«7Ñ2Ò2Ð2Ð2Ø�5‰5�‹8”v¤ Q£¨!¡¬T°!«W°q©[¸!Ð<Ü!% a£ ¨1¡¬d°1«g¸©k¸1Ð=ºyðJó Kò Kð Kð Kà�<‰<˜‹?œf Q›iÒ'Ð'Ñ'÷#-ñ -ú÷9ñ 9ús   ¨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  € ÜÓ!�J€A€qˆ!ˆQÜ	Ó	!ñ 2Ü�s˜A˜q°1·3±3¸¿¹±9Ø'(§s¡s¨Q¯S©S¡yô2ˆ÷2ð ×Ñ×$Ñ$ Q§\¡\Ó2°a·c±c¸A¿C¹C±iÒ?Ð?Ð?Ø�?‰?×#Ñ# A§L¡LÓ1°Q·S±S¸1¿3¹3±YÒ>Ð>Ð>äÓ!�J€A€qˆ!ˆQÜ	Ó	!ñ ;Ü  Q¨¸A¿C¹CÀ!Ç#Á#¹IØ01·±°a·c±c±	ô;ˆ÷;ð ×Ñ×'Ñ'¨¯©Ó5¸¿¹¸q¿s¹s¹ÒBÐBÐBØ×Ñ×&Ñ& q§|¡|Ó4¸¿¹¸a¿c¹c¹	Ò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6òR5ò"%AòP,?ò^I/òX5"òpLDò^"ò$Hò.Mò<Kò03,òla òH.'òb4+òn2ò6ò6)òDò2:3òz;3ò|%òP(ó.Br2   