§
    OŠtjBâ  ã                   ó†  — d dl mZ d dlmZ d dlmZ d dlmZ d dlm	Z	m
Z
 d dlmZmZmZmZ d dlmZ d dlmZmZmZmZmZmZmZmZmZmZmZ d d	lmZ d d
l m!Z!m"Z"m#Z# d dl$m%Z%m&Z& ej'        Z(d*d„Z)d„ Z*d„ Z+d„ Z,d„ Z-d„ Z.d„ Z/d„ Z0d„ Z1d„ Z2d„ Z3d„ Z4d„ Z5d„ Z6d„ Z7d„ Z8d„ Z9d„ Z:d„ Z;d „ Z<d!„ Z=d"„ Z>d#„ Z?d$„ Z@d%„ ZAd&„ ZBd'„ ZCd(„ ZDd)S )+é    )Ú
expand_mul)Úpi)ÚS)Úsqrt)ÚcosÚsin)ÚMatrixÚsimplifyÚeyeÚzeros)Úsymbols)ÚdynamicsymbolsÚ	RigidBodyÚParticleÚJointsMethodÚPinJointÚPrismaticJointÚCylindricalJointÚPlanarJointÚSphericalJointÚ	WeldJointÚBody©ÚJoint)ÚVectorÚReferenceFrameÚPoint)ÚraisesÚwarns_deprecated_sympyFc                 ój  — t          d¦  «        }t          d¦  «        }t          d|¬¦  «        }t          d|¬¦  «        }| rlt          d¦  «        t          d¦  «        }}|                     ||j        t          ¦  «         |                     ||j        t           dz  ¦  «         ||||||fS ||||fS )	NÚNÚAÚP©ÚframeÚCÚP_intÚC_inté   )r   r   Úorient_axisÚxr   Úy)Ú
interframer!   r"   r#   r&   ÚPintÚCints          úf/var/www/html/CA-Chatbot/venv/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_joint.pyÚ_generate_bodyr1      s¼   € Ý�sÑÔ€AÝ�sÑÔ€AÝ�#˜QÐÑÔ€AÝ�#˜QÐÑÔ€AØð &Ý# GÑ,Ô,­n¸WÑ.EÔ.EˆdˆØ×Ò˜˜AœC¥Ñ$Ô$Ð$Ø×Ò˜˜AœC¥"  q¡Ñ)Ô)Ð)Ø�!�Q˜˜4 Ð%Ð%Øˆa��Aˆ:Ðó    c                  óx   ‡ ‡— t          d¦  «        Št          d¦  «        Š t          t          ˆ ˆfd„¦  «         d S )NÚparentÚchildc                  ó&   •— t          d‰‰ ¦  «        S ©NÚJr   ©r5   r4   s   €€r0   ú<lambda>ztest_Joint.<locals>.<lambda>#   s   ø€ �e C¨°Ñ7Ô7€ r2   )r   r   Ú	TypeErrorr9   s   @@r0   Ú
test_Jointr<       sA   øø€ Ý�xÑ Ô €FÝ�gÑÔ€EÝ
�9Ð7Ð7Ð7Ð7Ð7Ñ8Ô8Ð8Ð8Ð8r2   c            	      ó„  ‡‡‡‡‡‡‡‡— t          d¦  «        \  } }}}t          d¦  «        \  }Š}}}}}	}
t          d¦  «        \  ŠŠ}}ŠŠ}}t          ¦   «         \  }}ŠŠt          d‰‰¦  «        Š‰                     | d¦  «        t	          | g¦  «        k    sJ ‚‰                     |gd¦  «        t	          |g¦  «        k    sJ ‚‰                     |gdd¬¦  «        t	          |g¦  «        k    sJ ‚‰                     d d¦  «        t	          |g¦  «        k    sJ ‚‰                     d gd¦  «        t	          |g¦  «        k    sJ ‚‰                     ‰d d gd¦  «        t	          ‰‰|g¦  «        k    sJ ‚‰                     d d¦  «        t	          |‰|g¦  «        k    sJ ‚‰                     g d¦  «        t	          |‰|g¦  «        k    sJ ‚‰                     g dd¬¦  «        t	          ‰||g¦  «        k    sJ ‚‰                     ‰d |gdd¬¦  «        t	          ‰||g¦  «        k    sJ ‚‰                     d dd¬¦  «        t	          ||g¦  «        k    sJ ‚‰                     d dd	¦  «        t	          |g¦  «        k    sJ ‚‰                     g dd	¦  «        t	          |||	g¦  «        k    sJ ‚‰                     d dd
¬¦  «        t	          |g¦  «        k    sJ ‚‰                     g dd	dd
¦  «        t	          |	g¦  «        k    sJ ‚‰                     g dd¦  «        t	          |‰|g¦  «        k    sJ ‚t          t          ˆˆˆfd„¦  «         t          t          ˆˆˆfd„¦  «         t          t          ˆˆˆfd„¦  «         t          t          ˆˆfd„¦  «         t          t          ˆˆˆfd„¦  «         t          ¦   «         \  }}ŠŠt          d‰‰‰‰                     t          ¦  «        ¦  «         t          ¦   «         \  }}ŠŠt          t          ˆˆˆfd„¦  «         t          t          ˆˆˆˆfd„¦  «         d S )Nzq u q_J u_Jzq0:4_J u0:4_Jz	q0:4 u0:4r8   é   r)   )Úoffseté   ÚuT)Únumber_singleÚqc                  ó4   •— ‰                       ‰‰gd¦  «        S )Nr>   ©Ú_fill_coordinate_list©r8   Úq0Úq1s   €€€r0   r:   z,test_coordinate_generation.<locals>.<lambda>F   ó   ø€ ˜q×6Ò6¸¸B°xÀÑCÔC€ r2   c                  ó8   •— ‰                       ‰‰d gdd¦  «        S )Nr)   rA   rE   )r8   Úu0Úu1s   €€€r0   r:   z,test_coordinate_generation.<locals>.<lambda>G   s   ø€ ˜q×6Ò6¸¸BÀ°~ÀqÈ#ÑNÔN€ r2   c                  ó4   •— ‰                       ‰‰gd¦  «        S )Nr@   rE   rG   s   €€€r0   r:   z,test_coordinate_generation.<locals>.<lambda>H   rJ   r2   c                  óN   •— ‰                       ‰t          d¦  «        gd¦  «        S )NrI   r)   )rF   r   )r8   rH   s   €€r0   r:   z,test_coordinate_generation.<locals>.<lambda>J   s"   ø€ ˜a×5Ò5°r½7À4¹=¼=Ð6IÈ1ÑMÔM€ r2   c                  ó:   •— ‰                       ‰‰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Ô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ÔK€ r2   c                  ó0   •— t          d‰‰ ‰‰‰g¬¦  «        S )Nr8   )ÚspeedsrR   )r&   r#   rL   rM   s   €€€€r0   r:   z,test_coordinate_generation.<locals>.<lambda>R   s   ø€ �~¨c°1°aÀÀRÈÀÐMÑMÔM€ r2   )
r   r1   r   rF   r	   r   Ú
ValueErrorr;   ÚdiffÚt)rC   rA   ÚqjÚujÚq0jÚq2jÚq3jÚu0jÚu1jÚu2jÚu3jÚq2Úq3Úu2Úu3Ú_r&   r8   r#   rH   rI   rS   rL   rM   s                   @@@@@@@@r0   Útest_coordinate_generationrg   &   s¿  øøøøøøøø€ Ý! -Ñ0Ô0�L€A€qˆ"ˆbÝ-;¸OÑ-LÔ-LÑ*€Cˆˆc�3˜˜S # sÝ%3°KÑ%@Ô%@Ñ"€BˆˆB��B˜˜B ÝÑ!Ô!�J€A€qˆ!ˆQå��a˜ÑÔ€Aà×"Ò" 1 aÑ(Ô(­F°A°3©K¬KÒ7Ð7Ð7Ð7Ø×"Ò" A 3¨Ñ*Ô*­f°a°S©k¬kÒ9Ð9Ð9Ð9Ø×"Ò" A 3¨°!Ð"Ñ4Ô4½À¸s¹¼ÒCÐCÐCÐCà×"Ò" 4¨Ñ+Ô+­v°r°d©|¬|Ò;Ð;Ð;Ð;Ø×"Ò" D 6¨1Ñ-Ô-µ¸¸±´Ò=Ð=Ð=Ð=Ø×"Ò" B¨¨dÐ#3°QÑ7Ô7½6Ø	ˆS�#ˆñ<ô <ò ð ð ð ð ×"Ò" 4¨Ñ+Ô+­v°s¸CÀ°oÑ/FÔ/FÒFÐFÐFÐFØ×"Ò" 2 qÑ)Ô)­V°S¸#¸s°OÑ-DÔ-DÒDÐDÐDÐDà×"Ò" 2 q°Ð"Ñ3Ô3µv¸sÀCÈ¸oÑ7NÔ7NÒNÐNÐNÐNØ×"Ò" B¨¨b >°1¸QÐ"Ñ?Ô?Å6Ø	ˆS�"ˆñDô Dò ð ð ð à×"Ò" 4¨°1Ð"Ñ5Ô5½ÀÀcÀ
Ñ9KÔ9KÒKÐKÐKÐKà×"Ò" 4¨¨CÑ0Ô0µF¸B¸4±L´LÒ@Ð@Ð@Ð@Ø×"Ò" 2 q¨#Ñ.Ô.µ&¸#¸sÀC¸Ñ2IÔ2IÒIÐIÐIÐIà×"Ò" 4¨¸$Ð"Ñ?Ô?Å6È3È%Á=Ä=ÒPÐPÐPÐPØ×"Ò" 2 q¨#¨q°$Ñ7Ô7½6À3À%¹=¼=ÒHÐHÐHÐHØ×"Ò" 2 q¨#Ñ.Ô.µ&¸#¸sÀC¸Ñ2IÔ2IÒIÐIÐIÐIå
�:ÐCÐCÐCÐCÐCÐCÑDÔDÐDÝ
�:ÐNÐNÐNÐNÐNÐNÑOÔOÐOÝ
�:ÐCÐCÐCÐCÐCÐCÑDÔDÐDå
�9ÐMÐMÐMÐMÐMÑNÔNÐNÝ
�9ÐGÐGÐGÐGÐGÐGÑHÔHÐHåÑ!Ô!�J€A€qˆ!ˆQÝˆS�!�Q˜˜BŸGšG¥A™JœJÑ'Ô'Ð'åÑ!Ô!�J€A€qˆ!ˆQÝ
�:ÐKÐKÐKÐKÐKÐKÑLÔLÐLÝ
�:ÐMÐMÐMÐMÐMÐMÐMÑNÔNÐNÐNÐNr2   c                  ó¨  — t          d¦  «        } t          d¦  «        }t          d¦  «        \  }}t          d¦  «        \  }}t          d| |¦  «        }|j        dk    sJ ‚|j        | k    sJ ‚|j        |k    sJ ‚|j        t          |g¦  «        k    sJ ‚|j	        t          |g¦  «        k    sJ ‚|j
        t          ||                     t          ¦  «        z
  g¦  «        k    sJ ‚|j        | j        j        k    sJ ‚|j                             |j        ¦  «        t'          d¦  «        k    sJ ‚|j                             | j        ¦  «        t'          d¦  «        k    sJ ‚|j                             |j        ¦  «        t'          d¦  «        k    sJ ‚|j                             | j        ¦  «        t'          d¦  «        k    sJ ‚|j        | j        k    sJ ‚|j        |j        k    sJ ‚|                     ¦   «         dk    sJ ‚t          d¦  «        }t          d	¦  «        }t3          d
¦  «        }	|	                     |j        |j        t8          dz  ¦  «         t          d||||j        j        z  ||j        j        z  |j        j        |	¬¦  «        }
|
j        |j        j        k    sJ ‚|
j                             |j        ¦  «        ||j        j        z  k    sJ ‚|
j                             |j        ¦  «        ||j        j        z  k    sJ ‚|
j                             |
j        ¦  «        t'          d¦  «        k    sJ ‚|j                             |j        ¦  «        | |j        j        z  ||j        j        z  z   k    sJ ‚|
j        |	k    sJ ‚|
j        |j        k    sJ ‚t          d¦  «        \  }}tA          d¦  «        \  }}} }}	}| j         !                    d|j        |j        z   ¦  «        }|j         !                    d|j        |j        z   ¦  «        }t          d| ||||||	||j        ¬¦
  «
        }|j        |j        k    sJ ‚|j         "                    |¦  «        dk    sJ ‚|j        |k    sJ ‚|j        |k    sJ ‚|j                             | j        ¦  «        |j        |j        z   k    sJ ‚|j                             |j        ¦  «        |j        |j        z   k    sJ ‚|j                             | j        ¦  «        |j        |j        z   |j        z
  |j        z
  k    sJ ‚|j         "                    |¦  «         #                    |¦  «        |tI          |¦  «        z  |tK          |¦  «        z  z
  |j        z  | tI          |¦  «        z  |tK          |¦  «        z  z
  |j        z  z   k    sJ ‚|j        |	k    sJ ‚|j        |k    sJ ‚d S )Nr#   r&   úl múq_J, u_Jr8   r   z PinJoint: J  parent: P  child: CÚP1ÚC1r'   r)   ÚJ1©Úparent_pointÚchild_pointÚ
joint_axisÚparent_interframeúq, uTro   rp   ©ro   rp   rr   Úchild_interframerq   )&r   r   r   r   Únamer4   r5   Úcoordinatesr	   rU   ÚkdesrW   rX   rq   r%   r+   rp   Úpos_fromÚ
masscenterr   ro   Ú_child_pointrr   ru   Ú__str__r   r*   r,   r   ÚzÚ_joint_axisÚ_parent_pointr1   Ú	locatenewÚvelÚexpressr   r   )r#   r&   ÚlÚmrC   rA   ÚPjrk   rl   r.   rm   r!   r"   r/   ro   rp   r8   s                    r0   Útest_pin_jointr†   U   s�  € Ý�#‰Œ€AÝ�#‰Œ€AÝ�5‰>Œ>�D€A€qÝ˜*Ñ%Ô%�D€A€qÝ	�#�q˜!Ñ	Ô	€BØŒ7�cŠ>ˆ>ˆ>ˆ>ØŒ9˜Š>ˆ>ˆ>ˆ>ØŒ8�qŠ=ˆ=ˆ=ˆ=ØŒ>�V Q C™[œ[Ò(Ð(Ð(Ð(ØŒ9� ˜s™œÒ#Ð#Ð#Ð#ØŒ7•f˜a !§&¢&­¡)¤)™m˜_Ñ-Ô-Ò-Ð-Ð-Ð-ØŒ=˜AœGœIÒ%Ð%Ð%Ð%ØŒ>×"Ò" 1¤<Ñ0Ô0µF¸1±I´IÒ=Ð=Ð=Ð=ØŒ?×#Ò# A¤LÑ1Ô1µV¸A±Y´YÒ>Ð>Ð>Ð>ØŒ?×#Ò# B¤OÑ4Ô4½¸q¹	¼	ÒAÐAÐAÐAØŒ<× Ò  ¤Ñ.Ô.µ&¸±)´)Ò;Ð;Ð;Ð;ØÔ 1¤7Ò*Ð*Ð*Ð*ØÔ !¤'Ò)Ð)Ð)Ð)Ø�:Š:‰<Œ<Ð=Ò=Ð=Ð=Ð=å	�4‰Œ€BÝ	�4‰Œ€BÝ˜'Ñ"Ô"€DØ×Ò�R”X˜rœt¥R¨!¡VÑ,Ô,Ð,Ý	�$˜˜B¨Q¨r¬x¬z©\Ø ¤¤
™l°r´x´zØ$(ð
*ñ 
*ô 
*€Bð Œ>˜RœXœZÒ'Ð'Ð'Ð'ØŒ?×#Ò# B¤MÑ2Ô2°a¸"¼(¼*±nÒDÐDÐDÐDØÔ×$Ò$ R¤]Ñ3Ô3°q¸2¼8¼:±~ÒEÐEÐEÐEØÔ×$Ò$ R¤_Ñ5Ô5½À¹¼ÒBÐBÐBÐBØŒM×"Ò" 2¤=Ñ1Ô1ØˆBˆrŒxŒz‰M˜A˜bœhœj™LÑ(ò)ð )ð )ð )àÔ 4Ò'Ð'Ð'Ð'ØÔ "¤(Ò*Ð*Ð*Ð*å˜&Ñ!Ô!�D€A€qÝ+¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dØ”<×)Ò)¨.¸!¼#ÀÄ¹)ÑDÔD€LØ”,×(Ò(¨¸¼¸a¼c¹	ÑBÔB€KÝ��a˜˜A˜q¨|Ø(¸DØ"&°1´3ð	8ñ 	8ô 	8€Að Œ<˜1œ3ÒÐÐÐØŒ>×Ò˜aÑ Ô  AÒ%Ð%Ð%Ð%ØŒ>˜\Ò)Ð)Ð)Ð)ØŒ=˜KÒ'Ð'Ð'Ð'ØŒ=×!Ò! !¤,Ñ/Ô/°1´3¸¼±9Ò<Ð<Ð<Ð<ØŒ>×"Ò" 1¤<Ñ0Ô0°A´C¸!¼#±IÒ=Ð=Ð=Ð=ØŒ<× Ò  ¤Ñ.Ô.°!´#¸¼±)¸a¼c±/ÀAÄCÑ2GÒGÐGÐGÐGØŒ<×Ò˜AÑÔ×&Ò& qÑ)Ô)¨aµ#°a±&´&©j¸1½sÀ1¹v¼v¹:Ñ.EÈÌÑ-LØˆB•�Q‘”‰K˜!�c !™fœ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        z  k    sJ ‚|                     |j        ¦  «        t          g d	¢d
t          |¦  «        t          |¦  «        gd
t          |¦  «         t          |¦  «        gg¦  «        k    sJ ‚|j	                             |j        ¦  «        | |z  |j        z  k    sJ ‚t          d¦  «        }t          d¦  «        }t          d¦  «        }t          d| ¬¦  «        }t          d¦  «        \  }}t	          d||||||||j        z  |j        ¬¦	  «	        }|j        |j        j        k    sJ ‚|                     |j        ¦  «        t          t          |¦  «        t          |¦  «        d
gt          |¦  «         t          |¦  «        d
gg d¢g¦  «        k    sJ ‚|j	                             |j        ¦  «        d
k    sJ ‚|j	                             |j        ¦  «        ||z  |j        z  k    sJ ‚t          d¦  «        \  }	}
}}}}d„ |	|f|
|f||ffD ¦   «         }t          d¦  «        t          d¦  «        }}t'          d||||¦  «        }|j                             |j        ¦  «        t)          d¦  «        k    sJ ‚|j	         
                    |j	        ¦  «        ||j        j        z  k    sJ ‚|j	                             |j        ¦  «        d
k    sJ ‚|j	                             |j        ¦  «        ||j        j        z  k    sJ ‚t          d¦  «        t          d¦  «        }}t          d¦  «        }t+          d|||	|
|||||j        z  |j        ¬¦
  «
        }|j                             |j        ¦  «        t          t          |	¦  «        d
t          |	¦  «        gg d¢t          |	¦  «         d
t          |	¦  «        gg¦  «        k    sJ ‚|j	         
                    |j	        ¦  «        ||j        z  |
|j        z  z   k    sJ ‚|j	                             |j        ¦  «        ||j        z  k    sJ ‚|j	                             |j        ¦  «                             |¦  «        | |j        z  ||z  |j        z  z
  k    sJ ‚t          d¦  «        t          d¦  «        }}t          d¦  «        }t/          d|||	|
|g|||g|||j        z  ¬¦	  «	        }|j        }|j                             |j        ¦  «        t          g d	¢d
t          |	¦  «        t          |	¦  «         gd
t          |	¦  «        t          |	¦  «        gg¦  «        k    sJ ‚|j	         
                    |j	        ¦  «        | |j        z  |
|j        z  z   ||j        z  z   k    sJ ‚|j	                             |j        ¦  «        ||z  |j        z  ||j        z  z   ||j        z  z   k    sJ ‚t          d¦  «        t          d¦  «        }}t          d¦  «        t          d¦  «        }}t1          d||||||j        z  ||j        z  ¬¦  «        }|                     |¦  «        t)          d¦  «        k    sJ ‚|j	         
                    |j	        ¦  «        ||j        z  ||j        z  z
  k    sJ ‚|j	                             |j        ¦  «        d
k    sJ ‚d S )Núm lr&   r#   )Úmassrs   r8   ©ru   rp   Ú	J_P_frame©r>   r   r   r   )rr   ru   rp   rq   ©r   r   r>   z	q1:4 u1:4c                 óJ   — i | ] \  }}|                      t          ¦  «        |“Œ!S © )rW   rX   )Ú.0ÚqiÚuis      r0   ú
<dictcomp>z/test_particle_compatibility.<locals>.<dictcomp>­   s(   € ÐMÐMÐM¡F B¨�—’�‘”˜RÐMÐMÐMr2   r@   ÚP_frame)rr   ro   rq   ©r   r>   r   ÚC_frame)rr   ru   ro   rp   )r   r   r   r   r   r,   ru   rr   rv   rz   ry   Údcmr	   r   r   r�   r}   rq   r+   r   r   r   Úxreplacer   r   )r„   rƒ   r–   r#   r&   rC   rA   r8   r”   rI   rb   rc   rM   rd   re   Ú	qdot_to_us                   r0   Útest_particle_compatibilityrš   Ž   s´  € Ý�5‰>Œ>�D€A€qÝ˜SÑ!Ô!€GÝ�‰Œ€AÝ�˜1ÐÑÔ€AÝ˜&Ñ!Ô!�D€A€qÝ��a˜˜A˜q°7Ø ¤™]ð	,ñ 	,ô 	,€AàÔ Ò(Ð(Ð(Ð(ØÔÔ# {Ò2Ð2Ð2Ð2ØŒ<× Ò  ¤Ñ.Ô.°1°"°w´y±.Ò@Ð@Ð@Ð@Ø�;Š;�qÔ*Ñ+Ô+­v°y°y°yØ89½3¸q¹6¼6Å3ÀqÁ6Ä6Ð7JØ89½CÀ¹F¼F¸7ÅCÈÁFÄFÐ7Kð7Mñ 0Nô 0Nò Nð Nð Nð Nð Œ<×Ò˜AÔ/Ñ0Ô0°Q°B¸±F¸W¼YÑ4FÒFÐFÐFÐFå˜SÑ!Ô!€GÝ˜SÑ!Ô!€GÝ�‰Œ€AÝ�˜1ÐÑÔ€AÝ˜&Ñ!Ô!�D€A€qÝ��a˜˜A˜q°GØ")°q¸7¼9±}Ø#œIð	'ñ 	'ô 	'€Að Œ<˜1Ô.Ô0Ò0Ð0Ð0Ð0Ø�;Š;�qÔ*Ñ+Ô+­v½¸A¹¼ÅÀAÁÄÈÐ7JÝ9<¸Q¹¼¸ÅÀQÁÄÈÐ7KØ7@°y°yð7Bñ 0Cô 0Cò Cð Cð Cð Cð Œ<×Ò˜AÔ/Ñ0Ô0°AÒ5Ð5Ð5Ð5ØŒ<×Ò˜AÔ/Ñ0Ô0°A¸±E¸G¼IÑ4EÒEÐEÐEÐEÝ+¨KÑ8Ô8Ñ€BˆˆB��B˜ØMÐM°°B¨x¸"¸b¸ÀBÈÀ8Ð.LÐMÑMÔM€Iå�C‰=Œ=�( 3™-œ-€q€AÝ�s˜A˜q ! QÑ'Ô'€AØÔ×"Ò" 1Ô#5Ñ6Ô6½#¸a¹&¼&Ò@Ð@Ð@Ð@ØŒ<× Ò  ¤Ñ.Ô.°!°aÔ6IÔ6KÑ2KÒKÐKÐKÐKØŒ<×Ò˜AÔ/Ñ0Ô0°AÒ5Ð5Ð5Ð5ØŒ<×Ò˜AÔ/Ñ0Ô0°A¸Ô8KÔ8MÑ4MÒMÐMÐMÐMå�C‰=Œ=�( 3™-œ-€q€AÝ˜YÑ'Ô'€GÝ˜˜a  B¨¨B°ÀgØ&'¨'¬)¡mÀÄ	ð	Kñ 	Kô 	K€AàÔ×"Ò" 1Ô#5Ñ6Ô6½&Ý	ˆR‰Œ�!•S˜‘W”WÐ˜y˜y˜y­C°©G¬G¨8°Q½¸B¹¼Ð*@ðBBñ ;Cô ;Cò Cð Cð Cð CàŒ<× Ò  ¤Ñ.Ô.°!°g´i±-À"ÀwÄyÁ.Ñ2PÒPÐPÐPÐPØŒ<×Ò˜AÔ/Ñ0Ô0°B¸¼±NÒBÐBÐBÐBØŒ<×Ò˜AÔ.Ñ/Ô/×8Ò8¸ÑCÔCØ	ˆˆgŒi‰˜!˜b™& 7¤9Ñ,Ñ,ò.ð .ð .ð .õ �C‰=Œ=�( 3™-œ-€q€AÝ˜YÑ'Ô'€GÝ�C˜˜A˜r B¨ 8¨R°"°b°Ø%,¸!¸g¼i¹-ð	Iñ 	Iô 	I€AàÔ!€GØÔ×"Ò" 1Ô#5Ñ6Ô6½&Øˆ	ˆ	�A•s˜2‘w”w¥ R¡¤ Ð)¨A­s°2©w¬w½¸B¹¼Ð+@ðBBñ ;Cô ;Cò Cð Cð Cð CàŒ<× Ò  ¤Ñ.Ô.Ø	
ˆˆWŒY‰˜˜gœi™Ñ'¨"¨w¬y©.Ñ8ò:ð :ð :ð :àŒ<×Ò˜AÔ/Ñ0Ô0Ø	ˆB‰�”Ñ˜R '¤)™^Ñ+¨b°7´9©nÑ<ò>ð >ð >ð >õ �C‰=Œ=�( 3™-œ-€q€AÝ% iÑ0Ô0µ.ÀÑ2KÔ2KˆW€GÝ�#�q˜!¨wØ#*¸¸W¼Y¹Ø '¤)™mð	-ñ 	-ô 	-€Að �;Š;�wÑÔ¥3 q¡6¤6Ò)Ð)Ð)Ð)ØŒ<× Ò  ¤Ñ.Ô.°!°g´i±-À!ÀgÄiÁ-Ñ2OÒOÐOÐOÐOØŒ<×Ò˜AÔ/Ñ0Ô0°AÒ5Ð5Ð5Ð5Ð5Ð5r2   c            
      ó  — t          d¦  «        \  } }t          d¦  «        }t          ¦   «         5  t          d¦  «        }t          d| |¬¦  «        }d d d ¦  «         n# 1 swxY w Y   t	          d¦  «        \  }}t          d||||||j        z  ¬¦  «         |j        |k    sJ ‚|j        j        dk    sJ ‚|j	         
                    |j	        ¦  «        | |j        z  k    sJ ‚|j                             |j        ¦  «        t          g d	¢d
t          |¦  «        t          |¦  «        gd
t          |¦  «         t          |¦  «        gg¦  «        k    sJ ‚|j	                             |j        ¦  «        | |z  |j        z  k    sJ ‚d S )Nrˆ   r&   r#   ©r‰   r%   rs   r8   ©rp   r”   rŒ   r   )r   r   r   r   r   r   r,   r%   rv   rz   ry   r—   r	   r   r   r�   r}   )r„   rƒ   r–   r#   r&   rC   rA   s          r0   Útest_body_compatibilityrž   ×   sÏ  € Ý�5‰>Œ>�D€A€qÝ˜SÑ!Ô!€GÝ	Ñ	!Ô	!ð -ð -Ý�‰IŒIˆÝ�˜1 GÐ,Ñ,Ô,ˆð-ð -ð -ñ -ô -ð -ð -ð -ð -ð -ð -øøøð -ð -ð -ð -õ ˜&Ñ!Ô!�D€A€qÝˆS�!�Q˜˜1¨!¨g¬i©-Ð8Ñ8Ô8Ð8ØŒ7�gÒÐÐÐØŒ7Œ<˜9Ò$Ð$Ð$Ð$ØŒ<× Ò  ¤Ñ.Ô.°1°"°q´s±(Ò:Ð:Ð:Ð:ØŒ7�;Š;�q”wÑÔ¥6¨9¨9¨9Ø,-­s°1©v¬vµs¸1±v´vÐ+>Ø,-µ°A±´¨w½¸A¹¼Ð+?ð+Añ $Bô $Bò Bð Bð Bð Bð Œ<×Ò˜AœGÑ$Ô$¨¨¨Q©°´©Ò4Ð4Ð4Ð4Ð4Ð4s   °"AÁA"Á%A"c            	      ó`  — t          d¦  «        \  } }t          d¦  «        \  }}t          d¦  «        \  }}t          d¦  «        }t          d¦  «        }t          d¦  «        }t          d|¬¦  «        }	t          d	||¬
¦  «        }
t          d||¬
¦  «        }t	          d|	|
|| | |j        z  |	j        j        ¬¦  «        }t	          d|
|||| |j        z  |
j        j        ¬¦  «        }|                     |¦  «        t          t          | ¦  «        t          | ¦  «         dgt          | ¦  «        t          | ¦  «        dgg d¢g¦  «        k    sJ ‚|                     |¦  «        t          t          |¦  «        t          |¦  «         dgt          |¦  «        t          |¦  «        dgg d¢g¦  «        k    sJ ‚t          |                     |¦  «        ¦  «        t          t          | |z   ¦  «        t          | |z   ¦  «         dgt          | |z   ¦  «        t          | |z   ¦  «        dgg d¢g¦  «        k    sJ ‚|                     |¦  «        ||j        z  k    sJ ‚|                     |¦  «        ||j        z  k    sJ ‚|                     |¦  «        ||j        z  ||j        z  z   k    sJ ‚|j        t          ||                      t           ¦  «        z
  g¦  «        k    sJ ‚|j        t          ||                     t           ¦  «        z
  g¦  «        k    sJ ‚|
j                             |¦  «        ||z  |j        z  k    sJ ‚|j                             |¦  «        ||z  |j        z  k    sJ ‚|j                             |¦  «        ||z  |j        z  |||z   z  |j        z  z   k    sJ ‚d S )Nzq1 q2zu1 u2rˆ   r!   r"   ÚBr&   r$   r#   ©r%   r‰   ÚRrm   )rU   rw   rp   rq   ÚJ2r   r�   )r   r   r   r   r   r+   r%   r}   r—   r	   r   r   r
   Ú
ang_vel_inrx   rW   rX   rz   r�   r,   )rI   rb   rM   rd   r„   rƒ   r!   r"   r    r&   ÚPartPÚPartRrm   r£   s                 r0   Útest_pin_joint_double_pendulumr§   è   s¶  € Ý˜GÑ$Ô$�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Œ8•v¥ B¡¤­#¨b©'¬'¨°1Ð5Ý # B¡¤­¨R©¬°!Ð4°i°i°iðAñ Bô Bò Bð Bð Bð Bà�5Š5�‰8Œ8•v¥ B¡¤­#¨b©'¬'¨°1Ð5Ý # B¡¤­¨R©¬°!Ð4°i°i°iðAñ Bô Bò Bð Bð Bð Bå�A—E’E˜!‘H”HÑÔ¥­#¨b°2©g©,¬,½¸RÀ"¹W¹¼¸ÀqÐ)IÝ25°b¸2±g±,´,ÅÀBÈÁGÁÄÈaÐ1PØ1:°°ð)<ñ "=ô "=ò =ð =ð =ð =ð
 �<Š<˜‰?Œ?˜b 1¤3™hÒ&Ð&Ð&Ð&Ø�<Š<˜‰?Œ?˜b 1¤3™hÒ&Ð&Ð&Ð&Ø�<Š<˜‰?Œ?˜b 1¤3™h¨¨a¬c©Ñ1Ò1Ð1Ð1Ð1ð Œ7•f˜b 2§7¢7­1¡:¤:™oÐ.Ñ/Ô/Ò/Ð/Ð/Ð/ØŒ7•f˜b 2§7¢7­1¡:¤:™oÐ.Ñ/Ô/Ò/Ð/Ð/Ð/ð Ô×Ò Ñ"Ô" a¨¡d¨1¬3¡hÒ.Ð.Ð.Ð.ØÔ×Ò Ñ"Ô" a¨¡d¨1¬3¡hÒ.Ð.Ð.Ð.ØÔ×Ò Ñ"Ô" a¨¡d¨1¬3¡h°°B¸±G±¸Q¼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        ¬¦  «        }|
                     |	¦  «        t          t          |¦  «        dt          |¦  «         gg d¢t          |¦  «        dt          |¦  «        gg¦  «        k    sJ ‚|
                     |¦  «        t          t          |¦  «        t          |¦  «         dgt          |¦  «        t          |¦  «        dgg d¢g¦  «        k    sJ ‚|                     |	¦  «        t          t          |¦  «        t          |¦  «        z  t          |¦  «        t          |¦  «         t          |¦  «        z  gt          |¦  «         t          |¦  «        z  t          |¦  «        t          |¦  «        t          |¦  «        z  gt          |¦  «        dt          |¦  «        gg¦  «        k    sJ ‚|
                     |	¦  «        ||	j        z  k    sJ ‚|
                     |¦  «        | |
j        z  k    sJ ‚|	                     |¦  «        | |	j        z  ||
j        z  z
  k    sJ ‚|j        t          ||                     t          ¦  «        z
  g¦  «        k    sJ ‚|j        t          ||                     t          ¦  «        z
  g¦  «        k    sJ ‚|j                             |j        ¦  «        ||
j        z  k    sJ ‚|j                             |j        ¦  «        | |
j        z  k    sJ ‚|j                             |	¦  «        |dz  |dz  z
  |z  |
j        z  k    sJ ‚|j                             |	¦  «        |dz  |dz  z
  |z  |dz  |dz  z   |z  z   |
j        z  k    sJ ‚d S )NzmA, mB, lA, lB, hztheta phi omega alphar!   r"   r    r)   é   Úrodr¡   Úplaterœ   r&   r$   rm   )rw   rU   rp   rq   r£   )rw   rU   ro   rq   r   r•   r�   )r   r   r   r   r   r}   r,   r—   r	   r   r   r¤   rx   rW   rX   rz   ry   r�   r+   )ÚmAÚmBÚlAÚlBÚhÚthetaÚphiÚomegaÚalphar!   r"   r    ÚlCrª   r«   r&   rm   r£   s                     r0   Útest_pin_joint_chaos_pendulumr¶     sX  € ÝÐ 3Ñ4Ô4Ñ€BˆˆB��AÝ-Ð.EÑFÔFÑ€Eˆ3��uÝ�sÑÔ€AÝ�sÑÔ€AÝ�sÑÔ€AØ
ˆq�1‰u‰*˜Ñ	€BØ
ˆQ‰$��1‘‰*€BÝ
�E ¨Ð
,Ñ
,Ô
,€CÝ�g B¨aÐ0Ñ0Ô0€EÝ�#˜QÐÑÔ€AÝ	�$˜˜3¨E¸%Ø  ¤™f°´ð
6ñ 
6ô 
6€Bå	�$˜˜U°¸EØ! !¤#™v°!´#ð
7ñ 
7ô 
7€Bð �5Š5�‰8Œ8•v¥ E¡
¤
¨Aµ°E±
´
¨{Ð;Ø(˜y˜yÝ # E¡
¤
¨A­s°5©z¬zÐ:ð<ñ =ô =ò =ð =ð =ð =ð �5Š5�‰8Œ8•v¥ C¡¤­3¨s©8¬8¨)°QÐ7Ý # C¡¤­#¨c©(¬(°AÐ6Ø(˜y˜yð*ñ +ô +ò +ð +ð +ð +ð �5Š5�‰8Œ8•vÝ	ˆS‰Œ•#�e‘*”*Ñ	�c #™hœh­¨U©¬¨µC¸±H´HÑ(<Ð=Ý
ˆc‰(Œ(ˆ•3�u‘:”:Ñ	�s 3™xœx­¨S©¬µ#°e±*´*Ñ)<Ð=Ý	ˆU‰Œ�Q�˜E™
œ
Ð#ð%ñ &ô &ò &ð &ð &ð &ð �<Š<˜‰?Œ?˜e A¤C™iÒ'Ð'Ð'Ð'Ø�<Š<˜‰?Œ?˜u˜f Q¤S™jÒ(Ð(Ð(Ð(Ø�<Š<˜‰?Œ?˜u˜f Q¤S™j¨5°´©9Ñ4Ò4Ð4Ð4Ð4ð Œ7•f˜e e§j¢jµ¡m¤mÑ3Ð4Ñ5Ô5Ò5Ð5Ð5Ð5ØŒ7•f˜e c§h¢h­q¡k¤kÑ1Ð2Ñ3Ô3Ò3Ð3Ð3Ð3ð Œ<× Ò  ¤Ñ0Ô0°B°q´s±FÒ:Ð:Ð:Ð:ØŒ>×"Ò" 5Ô#3Ñ4Ô4¸"¸¸q¼s¹
ÒBÐBÐBÐBð Œ>×Ò˜aÑ Ô  Q q¡S¨2¨a©4¡Z°Ñ$6°q´sÑ$:Ò:Ð:Ð:Ð:ØÔ×Ò Ñ"Ô"¨¨!©¨b°©d©
°EÑ'9Ø()¨!©¨b°©d©
°EÑ'9ñ(:Ø;<¼3ñ'?ò ?ð ?ð ?ð ?ð ?ð ?r2   c                  óˆ  ‡‡‡‡— t          d¦  «        \  } }t          ¦   «         \  }}ŠŠt          d¦  «        t          d¦  «        cŠŠt          t          ˆˆˆfd„¦  «         t          t          ˆˆˆfd„¦  «         ‰                     ||j        | ¦  «         ‰                     ||j        | ¦  «         t          t          ˆˆˆfd„¦  «         t          t          ˆˆˆfd„¦  «         t          ¦   «         \  }}ŠŠt          d¦  «        Š‰                     |t          dz  t          t          d	z  fd
¦  «         t          d‰‰| ||j
        ‰j         ‰‰j
        ¬¦	  «	         t          |                     |¦  «        ¦  «        t          dt          d	¦  «        t!          | ¦  «        z  dz  t          d	¦  «         t#          | ¦  «        z  dz  gt          d¦  «        dz  t          d¦  «        dt#          | ¦  «        z  t!          | ¦  «        z   z  dz  t          d¦  «        t#          | ¦  «         dt!          | ¦  «        z  z   z  dz  gt          d¦  «        dz  t          d¦  «        dt#          | ¦  «        z  t!          | ¦  «        z   z  dz  t          d¦  «         t#          | ¦  «        dt!          | ¦  «        z  z   z  dz  gg¦  «        z
  t%          d	¦  «        k    sJ ‚|                     |¦  «        |‰j
        z  k    sJ ‚‰j                             ‰j        ¦  «        |j
        |j        z   k    sJ ‚‰j                             |¦  «        ||j        z  k    sJ ‚‰j                             ‰¦  «        t/          d¦  «        k    sJ ‚‰j                             ‰¦  «        ||j        z  k    sJ ‚t          ¦   «         \  }}ŠŠt          d¦  «        Š‰                     |dt          z  d	z  t           t          dz  fd
¦  «         t          d‰‰| ||j         ‰j
        ‰‰j
        ‰j        z   ¬¦	  «	         t          |                     |¦  «        ¦  «        t          t          d¦  «         t#          | ¦  «        z  dz  t          d	¦  «         t!          | ¦  «        dz
  z  dz  t!          | ¦  «        dz  z
  t1          d¦  «        dz  z
  t          d	¦  «        t!          | ¦  «        dz   z  dz  t!          | ¦  «        dz  z
  t1          d¦  «        dz  z   gt!          | ¦  «        t          d¦  «        t          d¦  «        z   t#          | ¦  «         z  dz  t          d¦  «         t          d¦  «        z   t#          | ¦  «        z  dz  gt          d¦  «        t#          | ¦  «        z  dz  t          d	¦  «        t!          | ¦  «        dz   z  dz  t!          | ¦  «        dz  z   t1          d¦  «        dz  z
  t          d	¦  «        dt!          | ¦  «        z
  z  dz  t!          | ¦  «        dz  z   t1          d¦  «        dz  z   gg¦  «        k    sJ ‚|                     |¦  «        t          d¦  «        |z  dz  |j
        z  t          d¦  «        |z  dz  |j        z  z   k    sJ ‚‰j                             ‰j        ¦  «        |j         |j
        z
  k    sJ ‚‰j                             |¦  «                             ¦   «         t          d¦  «         t          d¦  «        z
  |z  dz  |j        z  t          d¦  «         t          d¦  «        z   |z  dz  |j        z  z   k    sJ ‚‰j                             ‰¦  «        t/          d¦  «        k    sJ ‚t          ¦   «         \  }}ŠŠt          d¦  «        t          d¦  «        cŠŠ‰                     |t           dz  t          t          dz  fd
¦  «         ‰                     |dt          z  d	z  t           t          dz  fd
¦  «         t          d‰‰| ||j
        |j        z
  ‰j         ‰‰‰j
        ‰j        z   ¬¦
  «
         t          |                     |¦  «        ¦  «        t          t!          | ¦  «        t          d¦  «        t          d¦  «        z   t#          | ¦  «         z  dz  t          d¦  «         t          d¦  «        z   t#          | ¦  «        z  dz  gt          d¦  «         t#          | ¦  «        z  dz  t          d	¦  «         t!          | ¦  «        dz   z  dz  t!          | ¦  «        dz  z
  t1          d¦  «        dz  z   t          d	¦  «        t!          | ¦  «        dz
  z  dz  t!          | ¦  «        dz  z
  t1          d¦  «        dz  z
  gt          d¦  «        t#          | ¦  «        z  dz  t          d	¦  «        t!          | ¦  «        dz
  z  dz  t!          | ¦  «        dz  z   t1          d¦  «        dz  z   t          d	¦  «         t!          | ¦  «        dz   z  dz  t!          | ¦  «        dz  z   t1          d¦  «        dz  z
  gg¦  «        k    sJ ‚|                     |¦  «        t          d¦  «        |z  dz  ‰j
        z  t          d¦  «        |z  dz  ‰j        z  z   k    sJ ‚‰j                             ‰j        ¦  «        |j
        |j        z
  |j        z   k    sJ ‚t          d¦  «         t          d¦  «        z   |z  dz  |j
        z  }‰j                             |¦  «                             ¦   «         |k    sJ ‚‰j                             ‰¦  «                             ¦   «         |k    sJ ‚‰j                             ‰¦  «        t/          d¦  «        k    sJ ‚d S )Nrs   r.   r/   c                  ó*   •— t          d‰‰ ‰¬¦  «        S ©Nr8   ©rr   ©r   ©r&   r#   r.   s   €€€r0   r:   z+test_pin_joint_interframe.<locals>.<lambda>D  ó   ø€ �x¨¨Q°ÀTÐJÑJÔJ€ r2   c                  ó*   •— t          d‰‰ ‰¬¦  «        S ©Nr8   ©ru   r»   ©r&   r/   r#   s   €€€r0   r:   z+test_pin_joint_interframe.<locals>.<lambda>E  ó   ø€ �x¨¨Q°ÀDÐIÑIÔI€ r2   c                  ó*   •— t          d‰‰ ‰¬¦  «        S r¹   r»   r¼   s   €€€r0   r:   z+test_pin_joint_interframe.<locals>.<lambda>I  r½   r2   c                  ó*   •— t          d‰‰ ‰¬¦  «        S r¿   r»   rÁ   s   €€€r0   r:   z+test_pin_joint_interframe.<locals>.<lambda>J  rÂ   r2   r©   r@   Úxyzr8   )ro   rp   rr   rq   g      à¿r)   é   éþÿÿÿr   )ro   rp   ru   rq   r>   rt   )r   r1   r   r   rV   r*   r}   Úorient_body_fixedr   r   r+   r,   r
   r—   r	   r   r   r   r   r¤   rz   ry   r�   r   r   )	rC   rA   r!   r"   ÚN_v_Cr&   r/   r#   r.   s	        @@@@r0   Útest_pin_joint_interframerÊ   ?  sÔ
  øøøø€ Ý˜&Ñ!Ô!�D€A€qåÑ!Ô!�J€A€qˆ!ˆQÝ Ñ'Ô'­¸Ñ)?Ô)?€J€Dˆ$Ý
�:ÐJÐJÐJÐJÐJÐJÑKÔKÐKÝ
�:ÐIÐIÐIÐIÐIÐIÑJÔJÐJà×Ò�Q˜œ˜QÑÔÐØ×Ò�Q˜œ˜QÑÔÐÝ
�:ÐJÐJÐJÐJÐJÐJÑKÔKÐKÝ
�:ÐIÐIÐIÐIÐIÐIÑJÔJÐJåÑ!Ô!�J€A€qˆ!ˆQÝ˜&Ñ!Ô!€DØ×Ò˜1�r A™v¥r­2°©6Ð2°EÑ:Ô:Ð:ÝˆS�!�Q˜˜1¨1¬3¸Q¼S¸DØ#°´ð8ñ 8ô 8ð 8å�A—E’E˜!‘H”HÑÔ¥Ø	•�a‘”�3˜q™6œ6Ñ! AÑ%­¨Q©¬ xµ#°a±&´&Ñ'8¸1Ñ'<Ð=Ý	ˆa‰Œ�1‰•d˜1‘g”g ¥S¨¡V¤V¡­c°!©f¬fÑ!4Ñ5¸Ñ9Ý	ˆa‰Œ•S˜‘V”V�G˜a¥# a¡&¤&™jÑ(Ñ	)¨AÑ	-ð	/å	ˆa‰Œ�1‰•d˜1‘g”g ¥c¨!¡f¤f¡­s°1©v¬vÑ!5Ñ6¸Ñ:Ý
ˆq‰'Œ'ˆ•S˜‘V”V˜a¥# a¡&¤&™jÑ(Ñ	)¨AÑ	-ð	/ð	(0ñ !1ô !1ñ 1õ
 5:¸!±H´Hò=ð =ð =ð =ð �<Š<˜‰?Œ?˜a $¤&™jÒ(Ð(Ð(Ð(ØŒ<× Ò  ¤Ñ.Ô.°!´#¸¼±)Ò;Ð;Ð;Ð;ØŒ<×Ò˜AÑÔ ! a¤c¡'Ò)Ð)Ð)Ð)ØŒ<×Ò˜DÑ!Ô!¥V¨A¡Y¤YÒ.Ð.Ð.Ð.ØŒ<×Ò˜DÑ!Ô! Q¨¬¡WÒ,Ð,Ð,Ð,åÑ!Ô!�J€A€qˆ!ˆQÝ˜&Ñ!Ô!€DØ×Ò˜1˜q¥2™v¨™z­B¨3µ°Q±Ð7¸Ñ?Ô?Ð?ÝˆS�!�Q˜˜1¨A¬C¨4¸Q¼SØ"¨q¬s°Q´S©yð:ñ :ô :ð :å�A—E’E˜!‘H”HÑÔ¥Ý
ˆq‰'Œ'ˆ•C˜‘F”FÑ	˜QÑ	Ý
ˆq‰'Œ'ˆ•S˜‘V”V˜a‘ZÑ	  1Ñ	$¥s¨1¡v¤v°¡zÑ	1µA°a±D´D¸1±HÑ	<Ý	ˆa‰Œ•C˜‘F”F˜Q‘JÑ	 !Ñ	#¥c¨!¡f¤f¨q¡jÑ	0µ1°Q±4´4¸!±8Ñ	;ð	=õ 
ˆQ‰Œ•$�q‘'”'�D ™GœGÑ#­¨A©¬ wÑ.°Ñ2Ý�‰7Œ7ˆ(•T˜!‘W”WÑ
¥ A¡¤Ñ	&¨Ñ	*ð	,å	ˆa‰Œ•3�q‘6”6Ñ	˜AÑ	Ý	ˆa‰Œ•C˜‘F”F˜Q‘JÑ	 !Ñ	#¥c¨!¡f¤f¨q¡jÑ	0µ1°Q±4´4¸!±8Ñ	;Ý	ˆa‰Œ�A�˜A™œ‘JÑ	 !Ñ	#¥c¨!¡f¤f¨q¡jÑ	0µ1°Q±4´4¸!±8Ñ	;ð	=ð)>ñ "?ô "?ò ?ð ?ð ?ð ?ð �<Š<˜‰?Œ?�d 1™gœg¨™k¨A™o°´Ñ3µd¸1±g´gÀ±kÀA±oÈÌÑ6KÑKÒKÐKÐKÐKØŒ<× Ò  ¤Ñ.Ô.°A´C°%¸!¼#±+Ò=Ð=Ð=Ð=ØŒ<×Ò˜AÑÔ×'Ò'Ñ)Ô)Ý	ˆa‰Œˆ•4˜‘7”7Ñ˜añ. Ø"#ñ.$Ø&'¤cñ.*å�Q‘”ˆx�$˜q™'œ'Ñ! Qñ-'Ø)*ñ-+Ø-.¬Sñ-1ñ.1ò 1ð 1ð 1ð 1ð Œ<×Ò˜DÑ!Ô!¥V¨A¡Y¤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°´©9À1Ä3À$Ø#°dØœ ¤™ð)ñ )ô )ð )õ �A—E’E˜!‘H”HÑÔ¥Ý	ˆQ‰Œ•$�q‘'”'�D ™GœGÑ#­¨A©¬ wÑ.°Ñ2Ý�‰7Œ7ˆ(•T˜!‘W”WÑ
¥ A¡¤Ñ	&¨Ñ	*ð	,å
ˆq‰'Œ'ˆ•C˜‘F”FÑ	˜QÑ	Ý
ˆq‰'Œ'ˆ•S˜‘V”V˜a‘ZÑ	  1Ñ	$¥s¨1¡v¤v°¡zÑ	1µA°a±D´D¸1±HÑ	<Ý	ˆa‰Œ•C˜‘F”F˜Q‘JÑ	 !Ñ	#¥c¨!¡f¤f¨q¡jÑ	0µ1°Q±4´4¸!±8Ñ	;ð	=õ 
ˆa‰Œ•3�q‘6”6Ñ	˜AÑ	Ý	ˆa‰Œ•C˜‘F”F˜Q‘JÑ	 !Ñ	#¥c¨!¡f¤f¨q¡jÑ	0µ1°Q±4´4¸!±8Ñ	;Ý
ˆq‰'Œ'ˆ•S˜‘V”V˜a‘ZÑ	  1Ñ	$¥s¨1¡v¤v°¡zÑ	1µA°a±D´D¸1±HÑ	<ð	>ð)?ñ "@ô "@ò @ð @ð @ð @ð �<Š<˜‰?Œ?�d 1™gœg¨™k¨A™o°´Ñ6½Ø	ñ:ô :Øñ:Øñ:Ø”Vñ:ñ ò ð ð ð àŒ<× Ò  ¤Ñ.Ô.°!´#¸¼±)¸a¼c±/ÒAÐAÐAÐAÝ�1‰gŒgˆX�˜Q™œÑ 1Ñ$ qÑ(¨1¬3Ñ.€EØŒ<×Ò˜AÑÔ×'Ò'Ñ)Ô)¨UÒ2Ð2Ð2Ð2ØŒ<×Ò˜DÑ!Ô!×*Ò*Ñ,Ô,°Ò5Ð5Ð5Ð5ØŒ<×Ò˜DÑ!Ô!¥V¨A¡Y¤YÒ.Ð.Ð.Ð.Ð.Ð.r2   c            
      óÈ  ‡‡‡‡‡‡— t          d¦  «        \  Š} t          d¦  «        \  Š}ŠŠŠŠt          d‰‰‰| ‰‰‰j        ¬¦  «        }|j        ‰j        k    sJ ‚‰                     |¦  «        t          t          ‰¦  «        dt          ‰¦  «        gg d¢t          ‰¦  «        dt          ‰¦  «         gg¦  «        k    sJ ‚t          d¦  «        \  Š}ŠŠŠŠt          d‰‰‰| ‰‰‰j        ¬¦  «        }|j        ‰j        k    sJ ‚‰                     |¦  «        t          t          ‰¦  «         dt          ‰¦  «        gg d¢t          ‰¦  «        dt          ‰¦  «        gg¦  «        k    sJ ‚t          ¦   «         \  Š}ŠŠt          d‰‰‰| ‰j	        ‰j	         ‰j	        ¬¦  «        }|j        ‰j	        k    sJ ‚‰                     |¦  «        t          t          ‰¦  «         t          ‰¦  «         dgt          ‰¦  «         t          ‰¦  «        dgg d¢g¦  «        k    sJ ‚t          ¦   «         \  Š}ŠŠt          d‰‰‰| ‰j	        ‰j	         ‰j
        ¬¦  «        }|j        ‰j
        k    sJ ‚‰                     |¦  «        t          g d¢dt          ‰¦  «        t          ‰¦  «        gdt          ‰¦  «        t          ‰¦  «         gg¦  «        k    sJ ‚t          d¦  «        \  Š}ŠŠŠŠt          t          ˆˆˆˆfd	„¦  «         t          t          ˆˆfd
„¦  «         t          t          ˆˆfd„¦  «         t          t          ˆˆˆˆfd„¦  «         t          t          ˆˆˆˆfd„¦  «         t          d¦  «        \  Š}ŠŠŠŠt          d‰‰‰‰‰j
        ‰j        z   ¬¦  «         t          t          ˆˆˆˆfd„¦  «         t          t          ˆˆˆˆfd„¦  «         d S )Nrs   Tr8   ©rr   ru   rq   r   ©r   éÿÿÿÿr   )r   r   rÎ   ©rÎ   r   r   c            	      ó„   •— t          d‰‰ t          ‰¦  «        ‰j        z  t          ‰¦  «        ‰j        z  z   ¬¦  «        S ©Nr8   )rq   )r   r   r+   r   r,   ©r&   r!   r#   rC   s   €€€€r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>ª  s?   ø€ �x¨¨Q°Ý36°q±6´6¸A¼C±<Å#ÀaÁ&Ä&È1Ì3Á,Ñ3Nð Pñ  Pô  P€ r2   c                  ó4   •— t          d‰‰ ‰ j        ¬¦  «        S rÑ   ©r   r+   ©r&   r#   s   €€r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>­  s   ø€ �x¨¨Q°¸a¼cÐBÑBÔB€ r2   c                  óD   •— t          d‰‰ ‰j        ‰ j        z   ¬¦  «        S rÑ   ©r   r+   r,   rÕ   s   €€r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>¯  s    ø€ �x¨¨Q°¸a¼cÀAÄC¹iÐHÑHÔH€ r2   c            	      óH   •— t          d‰‰ ‰‰‰j        ‰ j        z   ¬¦  «        S ©Nr8   rÌ   r×   ©r&   r/   r#   r.   s   €€€€r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>°  s,   ø€ �xØˆQ� T¸DØ”6˜AœC‘<ð !ñ  !ô  !€ r2   c            	      óH   •— t          d‰‰ ‰‰‰j        ‰j        z   ¬¦  «        S rÙ   r×   rÚ   s   €€€€r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>³  s,   ø€ �xØˆQ� T¸DØ”3˜œ‘<ð !ñ  !ô  !€ r2   c            
      óH   •— t          d‰‰ ‰‰t          d¦  «        ¬¦  «        S )Nr8   r   rÌ   )r   r   rÚ   s   €€€€r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>»  s*   ø€ �hØˆQ� T¸DÝ˜!‘9”9ðñ ô € r2   c            	      óH   •— t          d‰‰ ‰‰‰j        ‰j        z   ¬¦  «        S rÙ   )r   r,   rÚ   s   €€€€r0   r:   z+test_pin_joint_joint_axis.<locals>.<lambda>¾  s,   ø€ �hØˆQ� T¸DØ”3˜œ‘<ð!ñ !ô !€ r2   )r   r1   r   r,   rq   r—   r	   r   r   r}   r+   r   rV   Ú	Exception)	rA   r"   Úpinr&   r/   r!   r#   r.   rC   s	      @@@@@@r0   Útest_pin_joint_joint_axisrà   ‹  sØ  øøøøøø€ Ý˜&Ñ!Ô!�D€A€qå+¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ
�3˜˜1˜a °dØ$(°Q´Sð:ñ :ô :€CàŒ>˜QœSÒ Ð Ð Ð Ø�5Š5�‰8Œ8•v¥ A¡¤¨­3¨q©6¬6Ð2°J°J°JÝ # A¡¤¨­C°©F¬F¨7Ð3ð5ñ 6ô 6ò 6ð 6ð 6ð 6õ ,¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ
�3˜˜1˜a °dØ$(°T´Vð=ñ =ô =€CàŒ>˜TœVÒ#Ð#Ð#Ð#Ø�5Š5�‰8Œ8•v¥ Q¡¤ ¨­C°©F¬FÐ3°Z°Z°ZÝ # A¡¤¨­3¨q©6¬6Ð2ð4ñ 5ô 5ò 5ð 5ð 5ð 5õ  Ñ!Ô!�J€A€qˆ!ˆQÝ
�3˜˜1˜a °a´cØ%&¤S D°Q´Sð:ñ :ô :€CàŒ>˜QœSÒ Ð Ð Ð Ø�5Š5�‰8Œ8•v¥ Q¡¤ ­#¨a©&¬&¨°!Ð4½¸A¹¼°wÅÀAÁÄÈÐ6JØ)˜z˜zð+ñ ,ô ,ò ,ð ,ð ,ð ,åÑ!Ô!�J€A€qˆ!ˆQÝ
�3˜˜1˜a °a´cØ%&¤S D°Q´Sð:ñ :ô :€CàŒ>˜QœSÒ Ð Ð Ð Ø�5Š5�‰8Œ8•v˜z˜z˜z¨A­s°1©v¬vµs¸1±v´vÐ+>Ø !¥3 q¡6¤6­C°©F¬F¨7Ð3ð5ñ 6ô 6ò 6ð 6ð 6ð 6õ ,¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ
�:ð Pð Pð Pð Pð Pð Pð Pñ Qô Qð Qõ �:ÐBÐBÐBÐBÐBÑCÔCÐCå
�:ÐHÐHÐHÐHÐHÑIÔIÐIÝ
�:ð !ð !ð !ð !ð !ð !ð !ñ "ô "ð "õ �:ð !ð !ð !ð !ð !ð !ð !ñ "ô "ð "õ ,¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝˆS�!�Q¨$ÀØœ ¤™ð&ñ &ô &ð &õ �9ð ð ð ð ð ð ð ñ ô ð õ �9ð !ð !ð !ð !ð !ð !ð !ñ "ô "ð "ð "ð "r2   c                  ó2  — t          d¦  «        \  } }t          ¦   «         \  }}}}t          d|||j         ¬¦  «         |j                              |j        ¦  «        dk    sJ ‚|j                             |¦  «         |j        k    sJ ‚|                     |¦  «        t          g d¢dt          | ¦  «         t          | ¦  «         gdt          | ¦  «         t          | ¦  «        gg¦  «        k    sJ ‚| 
                    |¦  «        ||j        z  k    sJ ‚| 
                    |¦  «                             ¦   «         t          |dz  ¦  «        k    sJ ‚|j                             |j        ¦  «        dk    sJ ‚|j                             |j        ¦  «                             |¦  «                             ¦   «         dk    sJ ‚|j                             |¦  «        dk    sJ ‚t          ¦   «         \  }}}}t          d|||j        |j        ¬¦  «         |j                             |j        ¦  «        dk    sJ ‚|j                             |¦  «        |j        k    sJ ‚|                     |¦  «        t          dt          | ¦  «         t          | ¦  «         gg d¢dt          | ¦  «         t          | ¦  «        gg¦  «        k    sJ ‚| 
                    |¦  «        ||j        z  k    sJ ‚| 
                    |¦  «                             |¦  «        ||j        z  k    sJ ‚| 
                    |¦  «                             ¦   «         t          |dz  ¦  «        k    sJ ‚| 
                    |¦  «                             |j        ¦  «        dk    sJ ‚|j                             |¦  «        ||j        z  k    sJ ‚|j                             |j        ¦  «        |j         k    sJ ‚|j                             |j        ¦  «                             |¦  «                             ¦   «         t          | ¦  «        |j        z  t          | ¦  «        |j        z  z   k    sJ ‚|j                             |¦  «                             |j        ¦  «        t(          dz  k    sJ ‚t          ¦   «         \  }}}}t          d|||j        |j        ¬	¦  «         |j                             |j        ¦  «        dk    sJ ‚|j                             |¦  «        |j        k    sJ ‚|                     |¦  «        t          g d
¢t          | ¦  «         dt          | ¦  «        gt          | ¦  «        dt          | ¦  «        gg¦  «        k    sJ ‚| 
                    |¦  «        ||j        z  k    sJ ‚| 
                    |¦  «                             |¦  «        ||j        z  k    sJ ‚| 
                    |¦  «                             ¦   «         t          |dz  ¦  «        k    sJ ‚| 
                    |¦  «                             |j        ¦  «        }|                     |di¦  «        dk    sJ ‚|j                             |¦  «        dk    sJ ‚|j                             |j        ¦  «        |j        k    sJ ‚t          ¦   «         \  }}}}t          d|||j        |j        |j        |j        z   ¬¦  «         t-          |j                             |j        |j        z   ¦  «        ¦  «        dk    sJ ‚|j        |j        z                        |¦  «                             ¦   «         t          d¦  «        |j        z  k    sJ ‚t          |                     |¦  «        ¦  «        t          t          d¦  «        dz  t          d¦  «         t          | ¦  «        z  dz  t          d¦  «         t          | ¦  «        z  dz  gt          d¦  «        dz  t          d¦  «        t          | ¦  «        z  dz  t          d¦  «        t          | ¦  «        z  dz  gdt          | ¦  «         t          | ¦  «        gg¦  «        k    sJ ‚| 
                    |¦  «        ||j        z  k    sJ ‚| 
                    |¦  «                             |¦  «                             ¦   «         ||j        z  ||j        z  z   t          d¦  «        z  k    sJ ‚| 
                    |¦  «                             ¦   «         t          |dz  ¦  «        k    sJ ‚| 
                    |¦  «                             |j        |j        z   ¦  «        }|                     |di¦  «        dk    sJ ‚|j                             |¦  «                             ¦   «         ||j        z  t          d¦  «        z  k    sJ ‚|j                             |j        ¦  «        |j        |j        z
  k    sJ ‚|j                             |j        ¦  «                             |¦  «                             ¦   «         dt          d¦  «        dz  z
  |j        z  t          d¦  «        t          | ¦  «        z  dz  |j        z  z   t          d¦  «        t          | ¦  «        z  dz  |j        z  z   k    sJ ‚|j                             |¦  «                             |¦  «                             ¦   «         t          d¦  «         |z  t          | ¦  «        z  dz  |j        z  t          d¦  «        |z  t          | ¦  «        z  dz  |j        z  z   k    sJ ‚|j                             |¦  «                             |j        ¦  «        t(          dz  k    sJ ‚t          ¦   «         \  }}}}t          d|||j        |j        |j        |j        z   |j        z
  ¬¦  «         t-          |j                             |j        |j        z   |j        z
  ¦  «        ¦  «        dk    sJ ‚|j        |j        z   |j        z
                       |¦  «                             ¦   «         t          d¦  «        |j        z  k    sJ ‚t          |                     |¦  «        ¦  «        t          t          d¦  «        dz  t          d¦  «         t          | t(          dz  z   ¦  «        z  dz  t          d¦  «        t          | t(          dz  z   ¦  «        z  dz  gt          d¦  «        dz  t          d¦  «        t          | t(          dz  z   ¦  «        z  dz  t          d¦  «        t          | t(          dz  z   ¦  «        z  dz  gt          d¦  «         dz  t          d¦  «        t          | dt(          z  dz  z   ¦  «        z  dz  t          d¦  «        t          | dt(          z  dz  z   ¦  «        z  dz  gg¦  «        k    sJ ‚| 
                    |¦  «        ||j        z  k    sJ ‚| 
                    |¦  «                             |¦  «                             ¦   «         ||j        z  ||j        z  z   ||j        z  z
  t          d¦  «        z  k    sJ ‚| 
                    |¦  «                             ¦   «         t          |dz  ¦  «        k    sJ ‚| 
                    |¦  «                             |j        |j        z   |j        z
  ¦  «        }|                     |di¦  «                             ¦   «         dk    sJ ‚|j                             |¦  «                             ¦   «         ||j        z  ||j        z  z   t          d¦  «        z  k    sJ ‚|j                             |j        ¦  «        |j        |j        z
  k    sJ ‚|j                             |j        ¦  «                             |¦  «                             ¦   «         dt          d¦  «        dz  z
  |j        z  t          d¦  «        t          | t(          dz  z   ¦  «        z  dz  |j        z  z   t          d¦  «        t          | t(          dz  z   ¦  «        z  dz  |j        z  z
  k    sJ ‚|j                             |¦  «                             |¦  «                             ¦   «         t          d¦  «        |z  t          | t(          dz  z   ¦  «        z  dz  |j        z  t          d¦  «        |z  t          | t(          dz  z   ¦  «        z  dz  |j        z  z   k    sJ ‚|j                             |¦  «                             |j        ¦  «        t(          dz  k    sJ ‚t          ¦   «         \  }}}}t/          d¦  «        \  }}t          d||||j        z  ||j        z  |j        |j        z   |j        z
  |j        |j        z
  |j        z   ¬¦  «         |j        |j        z
  |j        z                        |j        |j        z   |j        z
  ¦  «        }t-          |¦  «        dk    sJ ‚|j        |j        z
  |j        z                        |¦  «                             ¦   «         dt          | ¦  «        z  dz  t1          d¦  «        dz  z
  |j        z  t1          d¦  «        dz  dt          | t(          dz  z   ¦  «        z  dz  z
  |j        z  z   dt          | t(          dz  z   ¦  «        z  dz  t1          d¦  «        dz  z
  |j        z  z   k    sJ ‚t          |                     |¦  «        ¦  «        t          t1          d¦  «        dz  dt          | ¦  «        z  dz  z
  dt          | t(          dz  z   ¦  «        z  dz  t1          d¦  «        dz  z
  dt          | t(          dz  z   ¦  «        z  dz  t1          d¦  «        dz  z   gdt          | t(          dz  z   ¦  «        z  dz  t1          d¦  «        dz  z   dt          | ¦  «        z  dz  t1          d¦  «        dz  z
  dt          | t(          dz  z   ¦  «        z  dz  t1          d¦  «        dz  z   gdt          | t(          dz  z   ¦  «        z  dz  t1          d¦  «        dz  z
  dt          | t(          dz  z   ¦  «        z  dz  t1          d¦  «        dz  z   dt          | ¦  «        z  dz  t1          d¦  «        dz  z
  gg¦  «        k    sJ ‚| 
                    |¦  «        ||j        z  ||j        z  z
  ||j        z  z   t          d¦  «        z  z
                       ¦   «         sJ ‚| 
                    |¦  «                             |¦  «                             ¦   «         ||j        z  ||j        z  z   ||j        z  z
  t          d¦  «        z  k    sJ ‚| 
                    |¦  «                             ¦   «         t          |dz  ¦  «        k    sJ ‚| 
                    |¦  «                             |j        |j        z   |j        z
  ¦  «        }|                     |di¦  «                             ¦   «         dk    sJ ‚|j                             |¦  «                             ¦   «         t          d¦  «        |z  |z  dz  |j        z  t          d¦  «        |z  |z  dz  |j        z  z   k    sJ ‚|j                             |j        ¦  «        ||j        z  ||j        z  z
  k    sJ ‚|j                             |j        ¦  «                             |¦  «                             ¦   «         ||dt          | ¦  «        z  dz
  z  dz  z   |j        z  |dt          | t(          dz  z   ¦  «        z  dz   z  dz  |j        z  z   |dt          | t(          dz  z   ¦  «        z  dz   z  dz  |j        z  z
  k    sJ ‚|j                             |¦  «                             |¦  «                             ¦   «         d|z  |z  t          | ¦  «        z  dz  |j        z  d|z  |z  t          | t(          dz  z   ¦  «        z  dz  |j        z  z   d|z  |z  t          | t(          dz  z   ¦  «        z  dz  |j        z  z   k    sJ ‚|j                             |¦  «                             |j        |j        z
  |j        z   ¦  «                             ¦   «         dk    sJ ‚d S )Nrj   r8   rÀ   r   rÏ   r)   rŠ   rŒ   ©rr   ro   r•   r>   ©ro   rp   ru   r@   rÆ   r©   é   é   úm n©ro   rp   ru   rr   éüÿÿÿrÇ   )r   r1   r   r+   Úangle_betweenr‚   r—   r	   r   r   r¤   Ú	magnituder   rz   ry   r
   r�   r,   Úcrossr}   r   r˜   r   r   r   Údot)	rC   rA   r!   r"   r#   r&   Úangler„   Úns	            r0   Útest_pin_joint_arbitrary_axisrï   Ã  st  € Ý˜*Ñ%Ô%�D€A€qõ  Ñ!Ô!�J€A€qˆ!ˆQÝˆS�!�Q¨!¬#¨Ð.Ñ.Ô.Ð.àŒSˆD×Ò ¤Ñ$Ô$¨Ò)Ð)Ð)Ð)ØŒC�KŠK˜‰NŒNˆ?˜aœcÒ!Ð!Ð!Ð!Ø�5Š5�‰8Œ8•v˜z˜z˜zØ !¥C¨¡F¤F 7­S°©V¬V¨GÐ4Ø !¥C¨¡F¤F 7­C°©F¬FÐ3ð5ñ 6ô 6ò 6ð 6ð 6ð 6ð �<Š<˜‰?Œ?˜a ¤™eÒ#Ð#Ð#Ð#Ø�<Š<˜‰?Œ?×$Ò$Ñ&Ô&­$¨q°!©t©*¬*Ò4Ð4Ð4Ð4ØŒ<× Ò  ¤Ñ.Ô.°!Ò3Ð3Ð3Ð3ØŒ<× Ò  ¤Ñ.Ô.×6Ò6°qÑ9Ô9×BÒBÑDÔDÈÒIÐIÐIÐIØŒ<×Ò˜AÑÔ !Ò#Ð#Ð#Ð#õ  Ñ!Ô!�J€A€qˆ!ˆQÝˆS�!�Q¨¬¸!¼#Ð>Ñ>Ô>Ð>àŒ3×Ò˜QœSÑ!Ô! QÒ&Ð&Ð&Ð&ØŒ3�;Š;�q‰>Œ>˜QœSÒ Ð Ð Ð Ø�5Š5�‰8Œ8•v ¥C¨¡F¤F 7­S°©V¬V¨GÐ4Ø(˜y˜yØ !¥C¨¡F¤F 7­C°©F¬FÐ3ð5ñ 6ô 6ò 6ð 6ð 6ð 6ð �<Š<˜‰?Œ?˜a ¤™eÒ#Ð#Ð#Ð#Ø�<Š<˜‰?Œ?×"Ò" 1Ñ%Ô%¨¨Q¬S©Ò0Ð0Ð0Ð0Ø�<Š<˜‰?Œ?×$Ò$Ñ&Ô&­$¨q°!©t©*¬*Ò4Ð4Ð4Ð4Ø�<Š<˜‰?Œ?× Ò  ¤Ñ%Ô%¨Ò*Ð*Ð*Ð*ØŒ<×Ò˜AÑÔ ! A¤C¡%Ò'Ð'Ð'Ð'ØŒ<× Ò  ¤Ñ.Ô.°1´3°$Ò6Ð6Ð6Ð6ØŒL×!Ò! !¤,Ñ/Ô/×7Ò7¸Ñ:Ô:×CÒCÑEÔEÝ�‰FŒF�1”3‰J�˜Q™œ ¤™Ñ#ò$ð $ð $ð $àŒ<×Ò˜AÑÔ×,Ò,¨Q¬SÑ1Ô1µR¸±TÒ9Ð9Ð9Ð9õ  Ñ!Ô!�J€A€qˆ!ˆQÝˆS�!�Q¨!¬#¸A¼CÐ@Ñ@Ô@Ð@àŒ3×Ò˜QœSÑ!Ô! QÒ&Ð&Ð&Ð&ØŒ3�;Š;�q‰>Œ>˜QœSÒ Ð Ð Ð Ø�5Š5�‰8Œ8•v˜y˜y˜yÝ!$ Q¡¤ ¨­C°©F¬FÐ3Ý # A¡¤¨­3¨q©6¬6Ð2ð4ñ 5ô 5ò 5ð 5ð 5ð 5ð �<Š<˜‰?Œ?˜a ¤™eÒ#Ð#Ð#Ð#Ø�<Š<˜‰?Œ?×"Ò" 1Ñ%Ô%¨¨1¬3©Ò.Ð.Ð.Ð.Ø�<Š<˜‰?Œ?×$Ò$Ñ&Ô&­$¨q°!©t©*¬*Ò4Ð4Ð4Ð4Ø�LŠL˜‰OŒO×)Ò)¨!¬#Ñ.Ô.€EØ�>Š>˜1˜a˜&Ñ!Ô! QÒ&Ð&Ð&Ð&ØŒ<×Ò˜AÑÔ !Ò#Ð#Ð#Ð#ØŒ<× Ò  ¤Ñ.Ô.°!´#Ò5Ð5Ð5Ð5õ  Ñ!Ô!�J€A€qˆ!ˆQÝˆS�!�Q Q¤S°a´cØœc A¤C™ið)ñ )ô )ð )å�a”c×'Ò'¨¬¨a¬c©	Ñ2Ô2Ñ3Ô3°qÒ8Ð8Ð8Ð8ØŒC�!”#‰I×Ò˜qÑ!Ô!×*Ò*Ñ,Ô,µ°Q±´¸¼±Ò;Ð;Ð;Ð;Ý�A—E’E˜!‘H”HÑÔ¥Ý	ˆa‰Œ�‰•T˜!‘W”W�H�S ™VœV‘O AÑ%­¨Q©¬ xµ°A±´¡°qÑ'8Ð9Ý	ˆa‰Œ�‰•D˜‘G”G�C ™FœF‘N 1Ñ$¥d¨1¡g¤g­c°!©f¬f¡n°QÑ&6Ð7Ø	
�S�‰VŒVˆG•S˜‘V”VÐð)ñ "ô "ò ð ð ð ð �<Š<˜‰?Œ?˜a ¤™eÒ#Ð#Ð#Ð#Ø�LŠL˜‰OŒO×#Ò# AÑ&Ô&×/Ò/Ñ1Ô1ØˆqŒs‰U�Q�q”s‘U‰]�D ™GœGÑ#ò$ð $ð $ð $à�<Š<˜‰?Œ?×$Ò$Ñ&Ô&­$¨q°!©t©*¬*Ò4Ð4Ð4Ð4Ø�LŠL˜‰OŒO×)Ò)¨!¬#°´©)Ñ4Ô4€EØ�>Š>˜1˜a˜&Ñ!Ô! QÒ&Ð&Ð&Ð&ØŒ<×Ò˜AÑÔ×'Ò'Ñ)Ô)¨a°!´#©gµt¸A±w´wÑ->Ò>Ð>Ð>Ð>ØŒ<× Ò  ¤Ñ.Ô.°!´#¸¼±)Ò;Ð;Ð;Ð;ØŒL×!Ò! !¤,Ñ/Ô/×7Ò7¸Ñ:Ô:×CÒCÑEÔEØ•�a‘”˜‘‰]˜AœCÑ¥$ q¡'¤'­#¨a©&¬&¡.°Ñ"2°1´3Ñ"6Ñ6Ý�‰GŒG•C˜‘F”F‰N˜1Ñ˜QœSÑ ñ!ò!ð !ð !ð !ð ŒL×Ò˜QÑÔ×'Ò'¨Ñ*Ô*×3Ò3Ñ5Ô5Ý�!‰WŒWˆH�Q‰J•s˜1‘v”vÑ˜aÑ ¤Ñ#¥d¨1¡g¤g¨a¡iµ°A±´Ñ&6°qÑ&8¸¼Ñ&<Ñ<ò=ð =ð =ð =àŒ<×Ò˜AÑÔ×,Ò,¨Q¬SÑ1Ô1µR¸±TÒ9Ð9Ð9Ð9åÑ!Ô!�J€A€qˆ!ˆQÝˆS�!�Q Q¤S°a´cØœc A¤C™i¨!¬#™oð/ñ /ô /ð /å�a”c×'Ò'¨¬¨a¬c©	°A´C©Ñ8Ô8Ñ9Ô9¸QÒ>Ð>Ð>Ð>ØŒC�!”#‰I˜œ‰O×$Ò$ QÑ'Ô'×0Ò0Ñ2Ô2µd¸1±g´g¸a¼c±kÒAÐAÐAÐAÝ�A—E’E˜!‘H”HÑÔ¥Ý	ˆa‰Œ�‰•T˜!‘W”W�H�S ¥R¨¡T¡™]œ]Ñ*¨1Ñ,Ý	ˆa‰Œ•�Q�˜A™‘X‘”Ñ	˜qÑ	 ð	"å	ˆa‰Œ�‰•D˜‘G”G�C ¥B r¡E¡	™NœNÑ*¨1Ñ,Ý	ˆa‰Œ•�Q�˜B™‘Y‘”Ñ	 Ñ	!ð	#å
ˆq‰'Œ'ˆ�!‰•T˜!‘W”W�S  Q¥r¡T¨"¡W¡Ñ-Ô-Ñ-¨aÑ/Ý	ˆa‰Œ•�Q˜�2™˜b™‘[Ñ!Ô!Ñ	! !Ñ	#ð	%ð)&ñ "'ô "'ò 'ð 'ð 'ð 'ð �<Š<˜‰?Œ?˜a ¤™eÒ#Ð#Ð#Ð#Ø�<Š<˜‰?Œ?×"Ò" 1Ñ%Ô%×.Ò.Ñ0Ô0°Q°q´s±U¸Q¸q¼s¹U±]Ø56°q´s±Uñ6;Ý<@À¹G¼Gñ5Dò Dð Dð Dð Dà�<Š<˜‰?Œ?×$Ò$Ñ&Ô&­$¨q°!©t©*¬*Ò4Ð4Ð4Ð4Ø�LŠL˜‰OŒO×)Ò)¨!¬#°´©)°A´C©-Ñ8Ô8€EØ�>Š>˜1˜a˜&Ñ!Ô!×*Ò*Ñ,Ô,°Ò1Ð1Ð1Ð1ØŒ<×Ò˜AÑÔ×'Ò'Ñ)Ô)¨a°´©e°a¸¼±e©m½TÀ!¹W¼WÑ-DÒDÐDÐDÐDØŒ<× Ò  ¤Ñ.Ô.°!´#¸¼±)Ò;Ð;Ð;Ð;ØŒL×!Ò! !¤,Ñ/Ô/×7Ò7¸Ñ:Ô:×CÒCÑEÔEØ•�a‘”˜‘‰]˜AœCÑ¥$ q¡'¤'­#¨aµ"°Q±$©h©-¬-Ñ"7¸Ñ"9¸!¼#Ñ"=Ñ=Ý�‰GŒG•C˜�B˜q™D™‘M”MÑ! !Ñ# A¤CÑ'ñ(ò(ð (ð (ð (ð ŒL×Ò˜QÑÔ×'Ò'¨Ñ*Ô*×3Ò3Ñ5Ô5Ý�‰GŒG�A‰I•c˜!�b ™d™(‘m”mÑ# AÑ% a¤cÑ)Ý�‰GŒG�A‰I•c˜!�b ™d™(‘m”mÑ# AÑ% a¤cÑ)ñ*ò*ð *ð *ð *ð Œ<×Ò˜AÑÔ×,Ò,¨Q¬SÑ1Ô1µR¸±TÒ9Ð9Ð9Ð9åÑ!Ô!�J€A€qˆ!ˆQÝ�5‰>Œ>�D€A€qÝˆS�!�Q Q¨¬¡W¸!¸a¼c¹'Øœc A¤C™i¨!¬#™oØ œs Q¤S™y¨1¬3™ð0ñ 0ô 0ð 0ð ŒS�1”3‰Y˜œ‰_×+Ò+¨A¬C°!´#©I¸¼©OÑ<Ô<€EÝ�eÑÔ Ò!Ð!Ð!Ð!ØŒS�”‰W�Q”S‰[×!Ò! !Ñ$Ô$×-Ò-Ñ/Ô/Ø•�A‘”‰Y�q‰[�1˜Q™4œ4 ™6Ñ! 1¤3Ñ&­!¨A©$¬$¨q©&°1µS¸½RÀ¹T¹±]´]±?À1Ñ3DÑ*DÀaÄcÑ)IÑIØ�s�1•r˜!‘t‘8‰}Œ}‰_˜QÑ¥ 1¡¤ a¡Ñ'¨¬Ñ,ñ-ò-ð -ð -ð -õ �A—E’E˜!‘H”HÑÔ¥Ý	
ˆ1‰Œˆa‰�!•C˜‘F”F‘(˜1‘*Ñ	˜b¥ Q­¨A©¡X¡¤Ñ.¨qÑ0µ1°Q±4´4¸±6Ñ9Ø	
�3ˆq•2�a‘4‰x‰=Œ=‰˜Ñ	�Q˜q™TœT !™VÑ	#ð	%à	
�3ˆq•2�a‘4‰x‰=Œ=‰˜Ñ	�Q˜q™TœT !™VÑ	# Q¥s¨1¡v¤v¡X¨a¡Zµ!°A±$´$°q±&Ñ%8Ø	
�3ˆq•2�a‘4‰x‰=Œ=‰˜Ñ	�Q˜q™TœT !™VÑ	#ð	%à	�C�•B�q‘D‘‰MŒMÑ	˜!Ñ	�a ™dœd 1™fÑ	$ a­¨Aµ°1±©H©¬¡o°aÑ&7½!¸A¹$¼$¸q¹&Ñ&@Ø	
�3ˆq‰6Œ6‰�!‰•a˜‘d”d˜1‘fÑ	ð	ð)ñ " ô " ò  ð  ð  ð  ð �LŠL˜‰OŒO˜q ¤™u q¨¬¡u™}¨q°´©uÑ4µd¸1±g´gÑ=Ñ=×GÒGÑIÔIÐIÐIÐIØ�<Š<˜‰?Œ?×"Ò" 1Ñ%Ô%×.Ò.Ñ0Ô0°Q°q´s±U¸Q¸q¼s¹U±]Ø56°q´s±Uñ6;Ý<@À¹G¼Gñ5Dò Dð Dð Dð Dà�<Š<˜‰?Œ?×$Ò$Ñ&Ô&­$¨q°!©t©*¬*Ò4Ð4Ð4Ð4Ø�LŠL˜‰OŒO×)Ò)¨!¬#¨a¬c©'°!´#©+Ñ6Ô6€EØ�>Š>˜1˜a˜&Ñ!Ô!×*Ò*Ñ,Ô,°Ò1Ð1Ð1Ð1ØŒL×Ò˜QÑÔ×(Ò(Ñ*Ô*Ý�‰GŒG�A‰I�a‰K˜‰M˜!œ#Ñ¥ Q¡¤¨¡	¨!¡¨A¡¨a¬cÑ 1Ñ1ò2ð 2ð 2ð 2àŒ<× Ò  ¤Ñ.Ô.°!°A´C±%¸!¸A¼C¹%±-Ò?Ð?Ð?Ð?ØŒL×!Ò! !¤,Ñ/Ô/×7Ò7¸Ñ:Ô:×CÒCÑEÔEØ��A•c˜!‘f”f‘H˜q‘LÑ! !Ñ#Ñ# Q¤SÑ(¨1¨aµ°A½¸1¹±H±´©oØñ/ñ ,Øñ,Ø”ñ,ñ Ø˜!�C ¥B q¡D¡™MœM™/¨AÑ-Ñ.¨qÑ0°´Ñ4ñ5ò5ð 5ð 5ð 5ð ŒL×Ò˜QÑÔ×'Ò'¨Ñ*Ô*×3Ò3Ñ5Ô5Ø�‰E�!‰G•C˜‘F”F‰N˜1Ñ˜QœSÑ  1 Q¡3 q¡5­¨Qµ°A±©X©¬Ñ#6°qÑ#8¸¼Ñ#<Ñ<Øˆa‰C�‰E•#�a�"˜Q™$‘h‘-”-Ñ Ñ! !¤#Ñ%ñ&ò&ð &ð &ð &ð Œ<×Ò˜AÑÔ×"Ò" 1¤3¨¬¡9¨q¬s¡?Ñ3Ô3×<Ò<Ñ>Ô>À!ÒCÐCÐCÐCÐCÐCr2   c                  ó¶  — t          ¦   «         \  } }}}t          j        ||j         |j        ¦  «        }|j        |j        k    sJ ‚t          j        ||j         |j        ¦  «        }|j        |j        k    sJ ‚t          j        ||j         |j        ¦  «        }|j        |j        k    sJ ‚t          j        ||j         |j        z
  |j        |j        z   ¦  «        }|j        |j        k    sJ ‚t          j        ||j         |j        z
  |j        |j        z   ¦  «        }|j        |j        k    sJ ‚t          j        ||j         |j        z
  |j        |j        z   ¦  «        }|j        |j        k    sJ ‚t          j        ||j         |j        z
  |j        z
  |j        |j        z   |j        z   ¦  «        }|j        |j        z
  |j        |j        z
  k    sJ ‚d S ©N)r1   r   Ú_create_aligned_interframer+   r}   r,   )r!   r"   r#   r&   Úfs        r0   Útest_create_aligned_frame_pirô   R  s•  € ÝÑ!Ô!�J€A€qˆ!ˆQÝÔ(¨¨Q¬S¨D°!´#Ñ6Ô6€AØŒ3�!”#Š:ˆ:ˆ:ˆ:ÝÔ(¨¨Q¬S¨D°!´#Ñ6Ô6€AØŒ3�!”#Š:ˆ:ˆ:ˆ:ÝÔ(¨¨Q¬S¨D°!´#Ñ6Ô6€AØŒ3�!”#Š:ˆ:ˆ:ˆ:ÝÔ(¨¨Q¬S¨D°1´3©J¸¼¸a¼c¹	ÑBÔB€AØŒ3�!”#Š:ˆ:ˆ:ˆ:ÝÔ(¨¨Q¬S¨D°1´3©J¸¼¸a¼c¹	ÑBÔB€AØŒ3�!”#Š:ˆ:ˆ:ˆ:ÝÔ(¨¨Q¬S¨D°1´3©J¸¼¸a¼c¹	ÑBÔB€AØŒ3�!”#Š:ˆ:ˆ:ˆ:ÝÔ(¨¨Q¬S¨D°1´3©J¸¼Ñ,<¸a¼cÀAÄC¹iÈ!Ì#¹oÑNÔN€AØŒ3�”‰9˜œ˜aœ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        ¬¦  «         ‰ 	                    |¦  «        |k    sJ ‚t          d¦  «        \  Š}ŠŠ}}t          d‰‰‰	| |||j         ¬¦  «         ‰ 	                    |¦  «        |k    sJ ‚t          d¦  «        \  Š}ŠŠ}}t          t          ˆˆˆˆ	fd„¦  «         d S )	Nzq uTr8   )rr   ru   r   rŒ   rÌ   c                  ó:   •— t          d‰‰ ‰‰j        z  ¬¦  «        S rÑ   )r   r}   rÒ   s   €€€€r0   r:   z%test_pin_joint_axis.<locals>.<lambda>x  s   ø€ �x¨¨Q°¸aÀ!Ä#¹gÐFÑFÔF€ r2   )r   r1   r   rq   r+   r	   r   r   r}   r—   r   rV   )
rA   r"   r.   r/   r8   ÚN_R_Ar&   r!   r#   rC   s
         @@@@r0   Útest_pin_joint_axisrø   d  s¯  øøøø€ Ý˜%Ñ Ô �D€A€qå+¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ��a˜˜A˜q°DÈ4ÐPÑPÔP€AØŒ<˜4œ6Ò!Ð!Ð!Ð!å�Q�˜A™œ¥ A¡¤Ð'Ø�˜Q™œ˜¥ Q¡¤Ð(Ø�I�Iðñ  ô  €Eõ ,¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝˆS�!�Q˜˜1°ÀtØœðñ ô ð à�5Š5�‰8Œ8�uÒÐÐÐÝ+¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝˆS�!�Q˜˜1°ÀtØœ˜ð!ñ !ô !ð !à�5Š5�‰8Œ8�uÒÐÐÐå+¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ
�:ÐFÐFÐFÐFÐFÐFÐFÑGÔGÐGÐGÐGr2   c                  óÎ  ‡‡‡‡‡— t          ¦   «         \  Š} ŠŠt          d‰‰‰j        ‰j        z   ¬¦  «        }|j        j        dk    sJ ‚|j                             ‰j        ¦  «        ‰j        ‰j        z   k    sJ ‚|j        ‰j        k    sJ ‚t          ¦   «         \  Š} ŠŠ‰j         	                    d‰j        ‰j        z   ¦  «        }t          d‰‰|‰j        ¬¦  «        }|j        |k    sJ ‚|j        ‰j        k    sJ ‚t          ¦   «         \  Š} ŠŠt          t          ˆˆˆfd„¦  «         t          d¦  «        Št          ¦   «         \  Š} ŠŠt          t          ˆˆˆˆfd„¦  «         t          ¦   «         \  Š} ŠŠ‰j         	                    d‰| j        z  ¦  «        Št          t          ˆˆˆfd	„¦  «         t          d¦  «        Št          t          ˆˆˆfd
„¦  «         d S )Nr8   ©ro   Ú	J_P_jointÚp)ro   rp   c                  óZ   •— t          d‰‰ ‰j                             ‰¦  «        ¬¦  «        S ©Nr8   rú   )r   r+   Ú	to_matrix)r&   r!   r#   s   €€€r0   r:   z'test_locate_joint_pos.<locals>.<lambda>Œ  s&   ø€ •8˜C  A°A´C·M²MÀ!Ñ4DÔ4DÐEÑEÔE€ r2   rC   c                  ó:   •— t          d‰‰ ‰‰j        z  ¬¦  «        S rþ   rÔ   rÒ   s   €€€€r0   r:   z'test_locate_joint_pos.<locals>.<lambda>�  s   ø€ �x¨¨Q°ÀÀAÄCÁÐHÑHÔH€ r2   c                  ó*   •— t          d‰‰ ‰¬¦  «        S ©Nr8   r�   r»   ©r&   r#   rp   s   €€€r0   r:   z'test_locate_joint_pos.<locals>.<lambda>“  ó   ø€ �x¨¨Q°¸{ÐKÑKÔK€ r2   c                  ó*   •— t          d‰‰ ‰¬¦  «        S r  r»   r  s   €€€r0   r:   z'test_locate_joint_pos.<locals>.<lambda>–  r  r2   )r1   r   r,   r}   ro   rv   ry   rz   rp   r€   r   r;   r   rV   r   )r"   Újointro   r&   r!   r#   rp   rC   s      @@@@@r0   Útest_locate_joint_posr  {  s  øøøøø€ åÑ!Ô!�J€A€qˆ!ˆQÝ�S˜!˜Q¨Q¬S°1´3©YÐ7Ñ7Ô7€EØÔÔ" kÒ1Ð1Ð1Ð1ØÔ×&Ò& q¤|Ñ4Ô4¸¼¸a¼c¹	ÒAÐAÐAÐAØÔ ¤Ò,Ð,Ð,Ð,åÑ!Ô!�J€A€qˆ!ˆQØ”<×)Ò)¨#¨q¬s°Q´S©yÑ9Ô9€LÝ�S˜!˜Q¨\Ø!"¤ð/ñ /ô /€EàÔ Ò-Ð-Ð-Ð-ØÔ ¤Ò,Ð,Ð,Ð,åÑ!Ô!�J€A€qˆ!ˆQÝ
�9ØEÐEÐEÐEÐEÐEñGô Gð Gõ 	�sÑÔ€AÝÑ!Ô!�J€A€qˆ!ˆQÝ
�:ÐHÐHÐHÐHÐHÐHÐHÑIÔIÐIÝÑ!Ô!�J€A€qˆ!ˆQØ”,×(Ò(¨¨a°!´#©gÑ6Ô6€KÝ
�:ÐKÐKÐKÐKÐKÐKÑLÔLÐLå˜‘*”*€KÝ
�:ÐKÐKÐKÐKÐKÐKÑLÔLÐLÐLÐLr2   c                  ó¢  ‡‡‡‡— t          ¦   «         \  } }ŠŠt          d¦  «        Š‰                     | | j        d¦  «         t	          d‰‰‰¬¦  «        }|j        ‰k    sJ ‚|j                             | ¦  «        dk    sJ ‚|j        |k    sJ ‚t          d¦  «        }t          ¦   «         \  } }ŠŠt          d¦  «        Š‰                     | | j        |¦  «         t          t          ˆˆˆfd„¦  «         t          ¦   «         \  } }ŠŠt          d¦  «        Š‰                     | | j        d¦  «         t          t          ˆˆˆfd„¦  «         d S )	NÚ	int_framer>   r8   rº   r   rC   c                  ó*   •— t          d‰‰ ‰¬¦  «        S r¹   r»   )r&   r#   rr   s   €€€r0   r:   z)test_locate_joint_frame.<locals>.<lambda>¨  s   ø€ •8˜C  AÐ9JÐKÑKÔ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ÑIÔI€ r2   )r1   r   r*   r}   r   rr   r¤   ru   r   r   rV   )r!   r"   r  rC   r&   r#   ru   rr   s       @@@@r0   Útest_locate_joint_framer  ™  s†  øøøø€ åÑ!Ô!�J€A€qˆ!ˆQÝ& {Ñ3Ô3ÐØ×!Ò! ! Q¤S¨!Ñ,Ô,Ð,Ý�S˜!˜QÐ2CÐDÑDÔD€EØÔ"Ð&7Ò7Ð7Ð7Ð7ØÔ"×-Ò-¨aÑ0Ô0°AÒ5Ð5Ð5Ð5ØÔ! QÒ&Ð&Ð&Ð&å�sÑÔ€AÝÑ!Ô!�J€A€qˆ!ˆQÝ& {Ñ3Ô3ÐØ×!Ò! ! Q¤S¨!Ñ,Ô,Ð,Ý
�:ØKÐKÐKÐKÐKÐKñMô Mð Mõ  Ñ!Ô!�J€A€qˆ!ˆQÝ% kÑ2Ô2ÐØ× Ò   A¤C¨Ñ+Ô+Ð+Ý
�:ØIÐIÐIÐIÐIÐIñKô Kð Kð Kð Kr2   c            	      óÊ  — t          ¦   «         \  } } }}t          d¦  «        \  }}t          d||¦  «        }|j        dk    sJ ‚|j        |k    sJ ‚|j        |k    sJ ‚|j        t          |g¦  «        k    sJ ‚|j        t          |g¦  «        k    sJ ‚|j	        t          || 
                    t          ¦  «        z
  g¦  «        k    sJ ‚|j        |j        j        k    sJ ‚|j                             |j        ¦  «        t%          d¦  «        k    sJ ‚|j                             |j        ¦  «        t%          d¦  «        k    sJ ‚|j                             |j        ¦  «        | |j        j        z  k    sJ ‚|j                             |j        ¦  «        | |j        j        z  k    sJ ‚|j                             |j        ¦  «        ||j        j        z  k    sJ ‚|j                             |j        ¦  «        dk    sJ ‚|j                             |j        ¦  «        dk    sJ ‚ |j        ¦   «         dk    sJ ‚t          ¦   «         \  }}}}t/          d¦  «        \  }}	t1          d¦  «        }
|
                     |j        |j        t6          dz  ¦  «         t          d||||j        j        z  |	|j        j        z  |j        j        |
¬¦  «        }|j        |j        j        k    sJ ‚|j                             |j        ¦  «        |	|j        j        z  k    sJ ‚|j                             |j        ¦  «        ||j        j        z  k    sJ ‚|j                             |j        ¦  «        | |j        j        z  k    sJ ‚|j                             |j        ¦  «        | |j        z  ||j        z  z
  |	|j        z  z   k    sJ ‚|j                             |j        ¦  «        ||j        j        z  k    sJ ‚|j                             |
¦  «        t%          d¦  «        k    sJ ‚|j                             |j        ¦  «        dk    sJ ‚|j                             |j        ¦  «        dk    sJ ‚t          ¦   «         \  } } }}t1          d¦  «        }
|
                     |j        |j        t6          dz  ¦  «         t          d||||j        j        z  |	|j        j        z  |j        j        |
¬¦  «        }|j        |j        j        k    sJ ‚|j                             |j        ¦  «        |	|j        j        z  k    sJ ‚|j                             |j        ¦  «        ||j        j        z  k    sJ ‚|j                             |j        ¦  «        | |j        j        z  k    sJ ‚|j                             |j        ¦  «        | |z
  |j        j        z  |	|j        j        z  z   k    sJ ‚|j                             |j        ¦  «        ||j        j        z  k    sJ ‚|j                             |j        ¦  «        dk    sJ ‚|j                             |j        ¦  «        dk    sJ ‚d S )	Núq_S, u_Sr   r   z&PrismaticJoint: S  parent: P  child: Cri   r'   r)   rn   )r1   r   r   rv   r4   r5   rw   r	   rU   rx   rW   rX   rq   r%   r+   rp   ry   rz   r   ro   r�   r¤   r|   r   r   r*   r,   r   r}   )rf   r#   r&   rC   rA   r   r!   r"   rƒ   r„   r.   s              r0   Útest_prismatic_jointr  ±  sÙ  € ÝÑ!Ô!�J€A€qˆ!ˆQÝ˜*Ñ%Ô%�D€A€qÝ�s˜A˜qÑ!Ô!€AØŒ6�SŠ=ˆ=ˆ=ˆ=ØŒ8�qŠ=ˆ=ˆ=ˆ=ØŒ7�aŠ<ˆ<ˆ<ˆ<ØŒ=�F A 3™KœKÒ'Ð'Ð'Ð'ØŒ8•v˜q˜c‘{”{Ò"Ð"Ð"Ð"ØŒ6•V˜Q §¢­¡¤™]˜OÑ,Ô,Ò,Ð,Ð,Ð,ØŒ<˜1œ7œ9Ò$Ð$Ð$Ð$ØŒ=×!Ò! !¤,Ñ/Ô/µ6¸!±9´9Ò<Ð<Ð<Ð<ØŒ>×"Ò" 1¤<Ñ0Ô0µF¸1±I´IÒ=Ð=Ð=Ð=ØŒ>×"Ò" 1¤=Ñ1Ô1°q°S¸1¼7¼9±_ÒDÐDÐDÐDØŒ<× Ò  ¤Ñ.Ô.°A°#¸¼¼	±/ÒAÐAÐAÐAØŒ<×Ò˜AœGÑ$Ô$¨¨A¬G¬I©Ò5Ð5Ð5Ð5ØŒ7×Ò˜aœgÑ&Ô&¨!Ò+Ð+Ð+Ð+ØŒ7×Ò˜aœgÑ&Ô&¨!Ò+Ð+Ð+Ð+Øˆ1Œ9‰;Œ;ÐBÒBÐBÐBÐBåÑ!Ô!�J€A€qˆ!ˆQÝ�5‰>Œ>�D€A€qÝ˜'Ñ"Ô"€DØ×Ò�Q”W˜aœc¥2¨¡6Ñ*Ô*Ð*Ý�s˜A˜q¨q°1´7´9©}Ø#$ q¤w¤y¡=¸Q¼W¼YØ)-ð	/ñ 	/ô 	/€Að Œ<˜1œ7œ9Ò$Ð$Ð$Ð$ØŒ=×!Ò! !¤,Ñ/Ô/°1°q´w´y±=Ò@Ð@Ð@Ð@ØŒ>×"Ò" 1¤<Ñ0Ô0°A¸¼¼	±MÒAÐAÐAÐAØŒ>×"Ò" 1¤=Ñ1Ô1°q°S¸1¼7¼9±_ÒDÐDÐDÐDØŒ<× Ò  ¤Ñ.Ô.°A°#¸¼±)¸aÀ!Ä#¹gÑ2EÈÈAÌCÉÑ2OÒOÐOÐOÐOØŒ<×Ò˜AœGÑ$Ô$¨¨A¬G¬I©Ò5Ð5Ð5Ð5ØŒ<×Ò˜DÑ!Ô!¥V¨A¡Y¤YÒ.Ð.Ð.Ð.ØŒ7×Ò˜aœgÑ&Ô&¨!Ò+Ð+Ð+Ð+ØŒ7×Ò˜aœgÑ&Ô&¨!Ò+Ð+Ð+Ð+åÑ!Ô!�J€A€qˆ!ˆQÝ˜'Ñ"Ô"€DØ×Ò�Q”W˜aœc¥2¨¡6Ñ*Ô*Ð*Ý�s˜A˜q¨q°1´7´9©}Ø#$ q¤w¤y¡=¸Q¼W¼YØ)-ð	/ñ 	/ô 	/€Að Œ<˜1œ7œ9Ò$Ð$Ð$Ð$ØŒ=×!Ò! !¤,Ñ/Ô/°1°q´w´y±=Ò@Ð@Ð@Ð@ØŒ>×"Ò" 1¤<Ñ0Ô0°A¸¼¼	±MÒAÐAÐAÐAØŒ>×"Ò" 1¤=Ñ1Ô1°q°S¸1¼7¼9±_ÒDÐDÐDÐDØŒ<× Ò  ¤Ñ.Ô.°A°2¸±6¸1¼7¼9Ñ2DÀqÈÌÌÁ{Ñ2RÒRÐRÐRÐRØŒ<×Ò˜AœGÑ$Ô$¨¨A¬G¬I©Ò5Ð5Ð5Ð5ØŒ7×Ò˜aœgÑ&Ô&¨!Ò+Ð+Ð+Ð+ØŒ7×Ò˜aœgÑ&Ô&¨!Ò+Ð+Ð+Ð+Ð+Ð+r2   c                  óª   — t          d¦  «        \  } }t          ¦   «         \  }}}}t          d|||j         ¬¦  «         |j                              |j        ¦  «        dk    sJ ‚|j                             |¦  «         |j        k    sJ ‚|                     |¦  «        t          g d¢g d¢g d¢g¦  «        k    sJ ‚|j         	                    |j        ¦  «        | |j        z  k    sJ ‚|j         	                    |j        ¦  «                             |¦  «         
                    ¦   «         |  |j        z  k    sJ ‚|j                             |¦  «        ||j        z  k    sJ ‚|j                             |¦  «                             |¦  «        | |j        z  k    sJ ‚|                     |¦  «        dk    sJ ‚|                     |¦  «        dk    sJ ‚t          ¦   «         \  }}}}t          d|||j        |j        ¬¦  «         |j                             |j        ¦  «        dk    sJ ‚|j                             |¦  «        |j        k    sJ ‚|                     |¦  «        t          g d¢g d	¢g d¢g¦  «        k    sJ ‚|j                             |¦  «        ||j        z  k    sJ ‚|j                             |¦  «                             |¦  «        ||j        z  k    sJ ‚|j         	                    |j        ¦  «        | |j        z  |j        z
  k    sJ ‚|j         	                    |j        ¦  «                             |¦  «         
                    ¦   «         | |j        z  |j        z   k    sJ ‚|                     |¦  «        dk    sJ ‚|                     |¦  «        dk    sJ ‚t          ¦   «         \  }}}}t          d|||j        |j        ¬
¦  «         |j                             |j        ¦  «        dk    sJ ‚|j                             |¦  «        |j        k    sJ ‚|                     |¦  «        t          g d¢g d¢g d¢g¦  «        k    sJ ‚|j                             |¦  «        ||j        z  k    sJ ‚|j                             |¦  «                             |¦  «        ||j        z  k    sJ ‚|j         	                    |j        ¦  «        |j        | |j        z  z   k    sJ ‚|                     |¦  «        dk    sJ ‚|                     |¦  «        dk    sJ ‚t          ¦   «         \  }}}}t          d|||j        |j        |j        |j        z   ¬¦  «         |j                             |j        |j        z   ¦  «        dk    sJ ‚|j        |j        z                        |¦  «        t          d¦  «        |j        z  k    sJ ‚|                     |¦  «        t          t          d¦  «        dz  t          d¦  «         dz  dgt          d¦  «        dz  t          d¦  «        dz  dgg d¢g¦  «        k    sJ ‚|j         	                    |j        ¦  «        | dz   |j        z  |j        z
  k    sJ ‚|j         	                    |j        ¦  «                             |¦  «        | t          d¦  «        dz  z
  dz   |j        z  t          d¦  «        dz  |j        z  z   k    sJ ‚|j                             |¦  «                             |¦  «        ||j        |j        z   z  t          d¦  «        z  k    sJ ‚|j                             |¦  «        ||j        z  k    sJ ‚|                     |¦  «        dk    sJ ‚|                     |¦  «        dk    sJ ‚t          ¦   «         \  }}}}t          d|||j        |j        |j        |j        z   |j        z
  ¬¦  «         |j                             |j        |j        z   |j        z
  ¦  «         
                    ¦   «         dk    sJ ‚|j        |j        z   |j        z
                       |¦  «        t          d¦  «        |j        z  z
   
                    ¦   «         dk    sJ ‚t          |                     |¦  «        ¦  «        t          t          d¦  «        dz  t          d¦  «         dz  t          d¦  «        dz  gt          d¦  «        dz  t          d¦  «        dz  t!          d¦  «        dz  z   t!          d¦  «        dz  t          d¦  «        dz  z
  gt          d¦  «         dz  t!          d¦  «        dz  t          d¦  «        dz  z
  t          d¦  «        dz  t!          d¦  «        dz  z   gg¦  «        k    sJ ‚|j         	                    |j        ¦  «        | dz   |j        z  |j        z
  k    sJ ‚|j         	                    |j        ¦  «                             |¦  «        | t          d¦  «        dz  z
  dz   |j        z  t          d¦  «        dz  |j        z  z   t          d¦  «        dz  |j        z  z
  z
   
                    ¦   «         dk    sJ ‚|j                             |¦  «        ||j        z  k    sJ ‚|j                             |¦  «                             |¦  «        t          d¦  «        |z  dz  |j        z  t          d¦  «        |z  dz  |j        z  z   t          d¦  «        |z  dz  |j        z  z
  z
   
                    ¦   «         sJ ‚|                     |¦  «        dk    sJ ‚|                     |¦  «        dk    sJ ‚t          ¦   «         \  }}}}t#          d¦  «        \  }}t          d||||j        z  ||j        z  |j        |j        z   |j        z
  |j        |j        z
  |j        z   ¬¦  «         |j        |j        z
  |j        z                        |j        |j        z   |j        z
  ¦  «         
                    ¦   «         dk    sJ ‚|j        |j        z   |j        z
                       |¦  «        |j        |j        z
  |j        z   z
   
                    ¦   «         dk    sJ ‚t          |                     |¦  «        ¦  «        t          t!          d¦  «         dz  t!          d¦  «         dz  t!          d¦  «        dz  gt!          d¦  «        dz  t!          d¦  «        dz  t!          d¦  «        dz  gt!          d¦  «         dz  t!          d¦  «        dz  t!          d¦  «        dz  gg¦  «        k    sJ ‚|j         	                    |j        ¦  «        |t          d¦  «        | z  dz  z   |j        z  t          d¦  «        | z  dz  |j        z  z
  t          d¦  «        | z  dz  |j        z  z   ||j        z  z
  z
                       |¦  «         
                    ¦   «         dk    sJ ‚|j         	                    |j        ¦  «                             |¦  «        ||dz  z   t          d¦  «        | z  dz  z   |j        z  d|z  dz  t          d¦  «        | z  dz  z
  |j        z  z   d|z  dz  t          d¦  «        | z  dz  z   |j        z  z   z
   
                    ¦   «         dk    sJ ‚|j                             |¦  «                             |¦  «        t          d¦  «        |z  dz  |j        z  t          d¦  «        |z  dz  |j        z  z
  t          d¦  «        |z  dz  |j        z  z   z
   
                    ¦   «         dk    sJ ‚|j                             |¦  «                             |¦  «        t          d¦  «        |z  dz  |j        z  t          d¦  «        |z  dz  |j        z  z   t          d¦  «        |z  dz  |j        z  z
  z
   
                    ¦   «         dk    sJ ‚|                     |¦  «        dk    sJ ‚|                     |¦  «        dk    sJ ‚d S )Nr  r   rÀ   r   rÏ   rÍ   r�   rŠ   rŒ   râ   r•   rã   r)   r>   r@   rÆ   ræ   rç   rÇ   )r   r1   r   r+   ré   r‚   r—   r	   rz   ry   r
   r�   r¤   r,   r   r}   r   r   )rC   rA   r!   r"   r#   r&   r„   rî   s           r0   Ú#test_prismatic_joint_arbitrary_axisr  ç  sB  € Ý˜*Ñ%Ô%�D€A€qåÑ!Ô!�J€A€qˆ!ˆQÝ�3˜˜1°´¨tÐ4Ñ4Ô4Ð4àŒSˆD×Ò ¤Ñ$Ô$¨Ò)Ð)Ð)Ð)ØŒC�KŠK˜‰NŒNˆ?˜aœcÒ!Ð!Ð!Ð!Ø�5Š5�‰8Œ8•v˜z˜z˜z¨:¨:¨:°y°y°yÐAÑBÔBÒBÐBÐBÐBØŒ<× Ò  ¤Ñ.Ô.°!°a´c±'Ò9Ð9Ð9Ð9ØŒ<× Ò  ¤Ñ.Ô.×6Ò6°qÑ9Ô9×BÒBÑDÔDÈÈÈQÌSÉÒPÐPÐPÐPØŒ<×Ò˜AÑÔ ! a¤c¡'Ò)Ð)Ð)Ð)ØŒ<×Ò˜AÑÔ×&Ò& qÑ)Ô)¨a¨R°!´#©XÒ5Ð5Ð5Ð5Ø�<Š<˜‰?Œ?˜aÒÐÐÐØ�<Š<˜‰?Œ?˜aÒÐÐÐõ  Ñ!Ô!�J€A€qˆ!ˆQÝ�3˜˜1¨q¬sÀÄÐDÑDÔDÐDàŒ3×Ò˜QœSÑ!Ô! QÒ&Ð&Ð&Ð&ØŒ3�;Š;�q‰>Œ>˜QœSÒ Ð Ð Ð Ø�5Š5�‰8Œ8•v˜z˜z˜z¨9¨9¨9°i°i°iÐ@ÑAÔAÒAÐAÐAÐAØŒ<×Ò˜AÑÔ ! a¤c¡'Ò)Ð)Ð)Ð)ØŒ<×Ò˜AÑÔ×&Ò& qÑ)Ô)¨Q°´©WÒ4Ð4Ð4Ð4ØŒ<× Ò  ¤Ñ.Ô.°!°A´C±%¸!¼#±+Ò=Ð=Ð=Ð=ØŒ<× Ò  ¤Ñ.Ô.×6Ò6°qÑ9Ô9×BÒBÑDÔDÈÈ!Ì#ÉÐPQÔPSÉÒSÐSÐSÐSØ�<Š<˜‰?Œ?˜aÒÐÐÐØ�<Š<˜‰?Œ?˜aÒÐÐÐõ  Ñ!Ô!�J€A€qˆ!ˆQÝ�3˜˜1°´À!Ä#ÐFÑFÔFÐFàŒ3×Ò˜QœSÑ!Ô! QÒ&Ð&Ð&Ð&ØŒ3�;Š;�q‰>Œ>˜aœcÒ!Ð!Ð!Ð!Ø�5Š5�‰8Œ8•v˜y˜y˜y¨*¨*¨*°i°i°iÐ@ÑAÔAÒAÐAÐAÐAØŒ<×Ò˜AÑÔ ! a¤c¡'Ò)Ð)Ð)Ð)ØŒ<×Ò˜AÑÔ×&Ò& qÑ)Ô)¨Q°´©WÒ4Ð4Ð4Ð4ØŒ<× Ò  ¤Ñ.Ô.°!´#¸¸!¼#¹±+Ò=Ð=Ð=Ð=Ø�<Š<˜‰?Œ?˜aÒÐÐÐØ�<Š<˜‰?Œ?˜aÒÐÐÐõ  Ñ!Ô!�J€A€qˆ!ˆQÝ�3˜˜1¨1¬3¸A¼CØ$%¤C¨!¬#¡Ið/ñ /ô /ð /àŒ3×Ò˜QœS 1¤3™YÑ'Ô'¨1Ò,Ð,Ð,Ð,ØŒC�!”#‰I×Ò˜qÑ!Ô!¥T¨!¡W¤W¨Q¬S¡[Ò0Ð0Ð0Ð0Ø�5Š5�‰8Œ8•v¥ Q¡¤¨¡	­D°©G¬G¨8°A©:°qÐ9½DÀ¹G¼GÀA¹IÅtÈAÁwÄwÈqÁyÐRSÐ;TÐV_ÐV_ÐV_Ð`ÑaÔaÒaÐaÐaÐaØŒ<× Ò  ¤Ñ.Ô.°1°q±5¸!¼#±+ÀÄÑ2CÒCÐCÐCÐCØŒ<× Ò  ¤Ñ.Ô.×6Ò6°qÑ9Ô9¸aÅ$ÀqÁ'Ä'È!Á)¹mÈaÑ>OÐQRÔQTÑ=TÕW[Ð\]ÑW^ÔW^Ð_`ÑW`ÐabÔadÑWdÑ=dÒdÐdÐdÐdØŒ<×Ò˜AÑÔ×&Ò& qÑ)Ô)¨Q°!´#¸¼±)©_½TÀ!¹W¼WÑ-DÒDÐDÐDÐDØŒ<×Ò˜AÑÔ ! A¤C¡%Ò'Ð'Ð'Ð'Ø�<Š<˜‰?Œ?˜aÒÐÐÐØ�<Š<˜‰?Œ?˜aÒÐÐÐåÑ!Ô!�J€A€qˆ!ˆQÝ�3˜˜1¨1¬3¸A¼CØ$%¤C¨!¬#¡I°´¡Oð5ñ 5ô 5ð 5àŒ3×Ò˜QœS 1¤3™Y¨¬™_Ñ-Ô-×6Ò6Ñ8Ô8¸AÒ=Ð=Ð=Ð=ØŒS�1”3‰Y˜œ‰_×%Ò% aÑ(Ô(­4°©7¬7°1´3©;Ñ6×@Ò@ÑBÔBÀaÒGÐGÐGÐGÝ�A—E’E˜!‘H”HÑÔ¥­$¨q©'¬'°!©)µd¸1±g´g°X¸a±ZÅÀaÁÄÈÁÐ)KÝ26°q±'´'¸!±)½TÀ!¹W¼WÀQ¹YÍÈ1ÉÌÈaÉÑ=OÕQRÐSTÑQUÔQUÐVWÑQWÕZ^Ð_`ÑZaÔZaÐbcÑZcÑQcÐ1dÝ37¸±7´7°(¸1±*½aÀ¹d¼dÀ1¹fÅtÈAÁwÄwÈqÁyÑ>PÕRVÐWXÑRYÔRYÐZ[ÑR[Õ^_Ð`aÑ^bÔ^bÐcdÑ^dÑRdÐ1eð)gñ "hô "hò hð hð hð hð Œ<× Ò  ¤Ñ.Ô.°1°q±5¸!¼#±+ÀÄÑ2CÒCÐCÐCÐCØŒL×!Ò! !¤,Ñ/Ô/×7Ò7¸Ñ:Ô:Ø
�d�1‰gŒg�a‰i‰-˜!Ñ
˜QœSÑ	 ¥4¨¡7¤7¨1¡9¨Q¬S¡=Ñ	0µ4¸±7´7¸1±9¸Q¼S±=Ñ	@ñBßCKÂ8Á:Ä:ÐQRòSð Sð Sð SàŒ<×Ò˜AÑÔ ! A¤C¡%Ò'Ð'Ð'Ð'ØŒL×Ò˜QÑÔ×'Ò'¨Ñ*Ô*ÝˆQ‰Œ�‰	�!‰�A”C‰�$˜q™'œ' !™) A™+ a¤c™/Ñ)­D°©G¬G°A©I°a©K¸¼©OÑ;ñ=ß>Fºh¹j¼jðIð Ið Ià�<Š<˜‰?Œ?˜aÒÐÐÐØ�<Š<˜‰?Œ?˜aÒÐÐÐåÑ!Ô!�J€A€qˆ!ˆQÝ�5‰>Œ>�D€A€qÝ�3˜˜1¨1¨Q¬S©5¸aÀÄ¹eØ$%¤C¨!¬#¡I°´¡OØ%&¤S¨1¬3¡Y°´¡_ð6ñ 6ô 6ð 6ð ŒC�”‰G�A”C‰K×&Ò& q¤s¨1¬3¡w¨q¬s¡{Ñ3Ô3×<Ò<Ñ>Ô>À!ÒCÐCÐCÐCØŒS�”‰W�Q”S‰[×!Ò! !Ñ$Ô$¨¬¨a¬c©	°A´C©Ñ8×BÒBÑDÔDÈÒIÐIÐIÐIÝ�A—E’E˜!‘H”HÑÔ¥­1¨Q©4¬4¨%°©'µA°a±D´D°5¸±7½A¸a¹D¼DÀ¹FÐ)CÝ23°A±$´$°q±&½!¸A¹$¼$¸q¹&Å!ÀAÁ$Ä$ÀqÁ&Ð1IÝ34°Q±4´4°%¸±'½1¸Q¹4¼4À¹6Å1ÀQÁ4Ä4ÈÁ6Ð1Jð)Lñ "Mô "Mò Mð Mð Mð Mð ŒL×!Ò! !¤,Ñ/Ô/Ø	
�T�!‰WŒW�Q‰Y�q‰[‰˜!œ#Ñ¥ Q¡¤¨¡	¨!¡¨A¬C¡Ñ/µ$°q±'´'¸!±)¸A±+¸a¼c±/ÑAÀAÀaÄcÁEÑIñKçŠg�a‰jŒjŸš™œ qò)ð )ð )ð )ð ŒL×!Ò! !¤,Ñ/Ô/×7Ò7¸Ñ:Ô:Ø	
ˆQˆq‰S‰•4˜‘7”7˜1‘9˜Q‘;Ñ	 ¤Ñ# q¨¡s¨1¡u­t°A©w¬w°q©y¸©{Ñ':¸A¼CÑ&?Ñ?Ø	ˆA‰ˆa‰•$�q‘'”'˜!‘)˜A‘+Ñ	˜qœsÑ"ñ	#ñ$ç%-¢X¡Z¤Z°1ò5ð 5ð 5ð 5ð ŒL×Ò˜QÑÔ×'Ò'¨Ñ*Ô*ÝˆQ‰Œ�‰	�!‰�A”C‰�$˜q™'œ' !™) A™+ a¤c™/Ñ)­D°©G¬G°A©I°a©K¸¼©OÑ;ñ=ß>Fºh¹j¼jÈAòNð Nð Nð NàŒL×Ò˜QÑÔ×'Ò'¨Ñ*Ô*Ý�!‰WŒW�Q‰Y�q‰[˜œ‰_�t A™wœw q™y¨™{¨1¬3™Ñ.µ°a±´¸±¸1±¸Q¼S±Ñ@ñBßCKÂ8Á:Ä:ÐQRòSð Sð Sð Sà�<Š<˜‰?Œ?˜aÒÐÐÐØ�<Š<˜‰?Œ?˜aÒÐÐÐÐÐr2   c                  óª	  — t          ¦   «         \  } }}}t          d¦  «        \  }}}}t          d||¦  «        }|j        dk    sJ ‚|j        |k    sJ ‚|j        |k    sJ ‚|j        t          ||g¦  «        k    sJ ‚|j        t          ||g¦  «        k    sJ ‚|j	        |k    sJ ‚|j
        |k    sJ ‚|j        |k    sJ ‚|j        |k    sJ ‚|j        t          ||                     t          ¦  «        z
  ||                     t          ¦  «        z
  g¦  «        k    sJ ‚|j        | j        k    sJ ‚|j                             |j        ¦  «        t+          d¦  «        k    sJ ‚|j                             |j        ¦  «        t+          d¦  «        k    sJ ‚|j                             |j        ¦  «        | | j        z  k    sJ ‚|j                             |j        ¦  «        || j        z  k    sJ ‚|j                             | ¦  «        || j        z  k    sJ ‚|                     | ¦  «        || j        z  k    sJ ‚|j        | k    sJ ‚|j        |k    sJ ‚|                     ¦   «         dk    sJ ‚t          d¦  «        \  }	}
}}t;          d¦  «        \  }}t          d¦  «        \  } }}}}}t          d|||	|||| j        z  ||j        z  ||d| j        z  ¬	¦  «        }|j        t          |	|g¦  «        k    sJ ‚|j        t          ||g¦  «        k    sJ ‚|j	        |	k    sJ ‚|j
        |k    sJ ‚|j        |k    sJ ‚|j        |k    sJ ‚|j        t          ||	                     t          ¦  «        z
  ||                     t          ¦  «        z
  g¦  «        k    sJ ‚|j        d| j        z  k    sJ ‚|j                             |j        ¦  «        ||j        z  k    sJ ‚|j                             |j        ¦  «        || j        z  k    sJ ‚|j                             |j        ¦  «        | | j        z  k    sJ ‚|j                             |j        ¦  «        || j        z  || j        z  z   ||j        z  z
  k    sJ ‚|j                             | ¦  «        || j        z  ||z  |j        z  z
  k    sJ ‚|                     | ¦  «        || j        z  k    sJ ‚d S )
Nzq0:2_J, u0:2_Jr8   r   z(CylindricalJoint: J  parent: P  child: Cz
q0:2, u0:2úl, mTr)   )Úrotation_coordinateÚrotation_speedÚtranslation_speedro   rp   rr   ru   rq   ) r1   r   r   rv   r4   r5   rw   r	   rU   r  Útranslation_coordinater  r  rx   rW   rX   rq   r+   rp   ry   rz   r   ro   r{   r�   r¤   rr   ru   r|   r   r,   r}   )r!   r"   r#   r&   Úq0_defÚq1_defÚu0_defÚu1_defÚCjrH   rI   rL   rM   rƒ   r„   r.   r/   s                    r0   Útest_cylindrical_jointr  K  s
  € ÝÑ!Ô!�J€A€qˆ!ˆQÝ%3Ð4DÑ%EÔ%EÑ"€FˆF�F˜FÝ	˜#˜q !Ñ	$Ô	$€BØŒ7�cŠ>ˆ>ˆ>ˆ>ØŒ9˜Š>ˆ>ˆ>ˆ>ØŒ8�qŠ=ˆ=ˆ=ˆ=ØŒ>�V V¨VÐ$4Ñ5Ô5Ò5Ð5Ð5Ð5ØŒ9� ¨Ð/Ñ0Ô0Ò0Ð0Ð0Ð0ØÔ! VÒ+Ð+Ð+Ð+ØÔ$¨Ò.Ð.Ð.Ð.ØÔ Ò&Ð&Ð&Ð&ØÔ 6Ò)Ð)Ð)Ð)ØŒ7•f˜f v§{¢{µ1¡~¤~Ñ5°vÀÇÂÍAÁÄÑ7NÐOÑPÔPÒPÐPÐPÐPØŒ=˜AœCÒÐÐÐØŒ>×"Ò" 1¤<Ñ0Ô0µF¸1±I´IÒ=Ð=Ð=Ð=ØŒ?×#Ò# A¤LÑ1Ô1µV¸A±Y´YÒ>Ð>Ð>Ð>ØŒ?×#Ò# B¤OÑ4Ô4¸¸À!Ä#¹ÒEÐEÐEÐEØŒ<× Ò  ¤Ñ.Ô.°&¸1¼3±,Ò>Ð>Ð>Ð>ØŒ>×Ò˜aÑ Ô  F¨Q¬S¡LÒ0Ð0Ð0Ð0Ø�<Š<˜‰?Œ?˜f q¤s™lÒ*Ð*Ð*Ð*ØÔ 1Ò$Ð$Ð$Ð$ØÔ !Ò#Ð#Ð#Ð#Ø�:Š:‰<Œ<ÐEÒEÐEÐEÐEå# LÑ1Ô1�N€BˆˆB�Ý�6‰?Œ?�D€A€qÝ+¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ	˜#˜q !¸ÈBØ,.¸QÀÄ¹WØ&'¨!¬#¡gÀØ+/¸AÀÄ¹Gð
Eñ 
Eô 
E€Bð Œ>�V R¨ LÑ1Ô1Ò1Ð1Ð1Ð1ØŒ9�  B˜xÑ(Ô(Ò(Ð(Ð(Ð(ØÔ! RÒ'Ð'Ð'Ð'ØÔ$¨Ò.Ð.Ð.Ð.ØÔ Ò"Ð"Ð"Ð"ØÔ 2Ò%Ð%Ð%Ð%ØŒ7•f˜b 2§7¢7­1¡:¤:™o¨r°F·K²KÅ±N´NÑ/BÐCÑDÔDÒDÐDÐDÐDØŒ=˜A ¤™GÒ#Ð#Ð#Ð#ØŒ>×"Ò" 1¤<Ñ0Ô0°A¸¼±GÒ;Ð;Ð;Ð;ØŒ?×#Ò# A¤LÑ1Ô1°Q¸¼±WÒ<Ð<Ð<Ð<ØŒ?×#Ò# B¤OÑ4Ô4¸¸À!Ä#¹ÒEÐEÐEÐEØŒ<× Ò Ø	Œñô Ø˜QœS™ 6¨A¬C¡<Ñ/°!°a´c±'Ñ9ò:ð :ð :ð :àŒ<×Ò˜AÑÔ " q¤s¡(¨R°!©V°a´c©\Ñ"9Ò9Ð9Ð9Ð9Ø�<Š<˜‰?Œ?˜b 1¤3™hÒ&Ð&Ð&Ð&Ð&Ð&r2   c                  óx  — t          ¦   «         \  } }}}t          d¦  «        \  }}}t          d¦  «        \  }}}	t          d||¦  «        }
|
j        dk    sJ ‚|
j        |k    sJ ‚|
j        |k    sJ ‚|
j        t          |||g¦  «        k    sJ ‚|
j        t          |||	g¦  «        k    sJ ‚|
j	        |k    sJ ‚|
j
        t          ||g¦  «        k    sJ ‚|
j        |k    sJ ‚|
j        t          ||	g¦  «        k    sJ ‚|
j        t          ||                     t          ¦  «        z
  ||                     t          ¦  «        z
  |	|                     t          ¦  «        z
  g¦  «        k    sJ ‚|
j        | j        k    sJ ‚|
j        | j        | j        gk    sJ ‚|
j                             |j        ¦  «        t1          d¦  «        k    sJ ‚|
j                             |j        ¦  «        t1          d¦  «        k    sJ ‚|| j        z  || j        z  z   }|
j                             |
j        ¦  «        | k    sJ ‚|j                             |j        ¦  «        |k    sJ ‚|
j                             | ¦  «        || j        z  |	| j        z  z   k    sJ ‚|                     | ¦  «        || j        z  k    sJ ‚|
j        | k    sJ ‚|
j        |k    sJ ‚|
                     ¦   «         dk    sJ ‚t          d¦  «        \  }}}}}}t?          d¦  «        \  }}t          d¦  «        \  } }}}}}t          d|||||g||g|| j        z  ||j        z  ||¬	¦
  «
        }
|
j        t          |||g¦  «        k    sJ ‚|
j        t          |||g¦  «        k    sJ ‚|
j	        |k    sJ ‚|
j
        t          ||g¦  «        k    sJ ‚|
j        |k    sJ ‚|
j        t          ||g¦  «        k    sJ ‚|
j        t          ||                     t          ¦  «        z
  ||                     t          ¦  «        z
  ||                     t          ¦  «        z
  g¦  «        k    sJ ‚|
j        |j        k    sJ ‚|
j        |j        |j        gk    sJ ‚|
j                             |j        ¦  «        ||j        z  k    sJ ‚|
j                             |j        ¦  «        || j        z  k    sJ ‚|
j                             |
j        ¦  «        || j        z  || j        z  z   k    sJ ‚|j                             |j        ¦  «        || j        z  || j        z  z
  || j        z  z
  ||j        z  z
  k    sJ ‚|j                             | ¦  «        | | j        z  || j        z  z
  ||z  |j        z  z   k    sJ ‚|                     | ¦  «        || j        z  k    sJ ‚d S )
Nzq0:3_Jzu0:3_Jr8   r   z#PlanarJoint: J  parent: P  child: Cú
q0:3, u0:3r  T)r  Úplanar_coordinatesÚplanar_speedsro   rp   rr   ru   ) r1   r   r   rv   r4   r5   rw   r	   rU   r  r   r  r!  rx   rW   rX   Úrotation_axisr+   Úplanar_vectorsr,   r}   rp   ry   rz   r   ro   r�   r¤   rr   ru   r|   r   )r!   r"   r#   r&   r  r  Úq2_defr  r  Úu2_defr  Úr_P_CrH   rI   rb   rL   rM   rd   rƒ   r„   r.   r/   s                         r0   Útest_planar_jointr'  |  s  € ÝÑ!Ô!�J€A€qˆ!ˆQÝ+¨HÑ5Ô5Ñ€FˆF�FÝ+¨HÑ5Ô5Ñ€FˆF�FÝ	�S˜!˜QÑ	Ô	€BØŒ7�cŠ>ˆ>ˆ>ˆ>ØŒ9˜Š>ˆ>ˆ>ˆ>ØŒ8�qŠ=ˆ=ˆ=ˆ=ØŒ>�V V¨V°VÐ$<Ñ=Ô=Ò=Ð=Ð=Ð=ØŒ9� ¨°Ð7Ñ8Ô8Ò8Ð8Ð8Ð8ØÔ! VÒ+Ð+Ð+Ð+ØÔ ¥F¨F°FÐ+;Ñ$<Ô$<Ò<Ð<Ð<Ð<ØÔ Ò&Ð&Ð&Ð&ØÔ�v v¨vÐ&6Ñ7Ô7Ò7Ð7Ð7Ð7ØŒ7•f˜f v§{¢{µ1¡~¤~Ñ5°vÀÇÂÍAÁÄÑ7NØ$ v§{¢{µ1¡~¤~Ñ5ð7ñ 8ô 8ò 8ð 8ð 8ð 8àÔ˜qœsÒ"Ð"Ð"Ð"ØÔ ¤ a¤c 
Ò*Ð*Ð*Ð*ØŒ>×"Ò" 1¤<Ñ0Ô0µF¸1±I´IÒ=Ð=Ð=Ð=ØŒ?×#Ò# A¤LÑ1Ô1µV¸A±Y´YÒ>Ð>Ð>Ð>Ø�Q”S‰L˜6 A¤C™<Ñ'€EØŒ?×#Ò# B¤NÑ3Ô3¸°vÒ=Ð=Ð=Ð=ØŒ<× Ò  ¤Ñ.Ô.°%Ò7Ð7Ð7Ð7ØŒ>×Ò˜aÑ Ô  F¨Q¬S¡L°6¸A¼C±<Ñ$?Ò?Ð?Ð?Ð?Ø�<Š<˜‰?Œ?˜f q¤s™lÒ*Ð*Ð*Ð*ØÔ 1Ò$Ð$Ð$Ð$ØÔ !Ò#Ð#Ð#Ð#Ø�:Š:‰<Œ<Ð@Ò@Ð@Ð@Ð@å+¨LÑ9Ô9Ñ€BˆˆB��B˜Ý�6‰?Œ?�D€A€qÝ+¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ	�S˜!˜Q°BØ)+¨R¨ÀÀRÀØ"# a¤c¡'°q¸1¼3±wØ'+¸dð
Dñ 
Dô 
D€Bð Œ>�V R¨¨R LÑ1Ô1Ò1Ð1Ð1Ð1ØŒ9� ¨¨BÐ/Ñ0Ô0Ò0Ð0Ð0Ð0ØÔ! RÒ'Ð'Ð'Ð'ØÔ ¥F¨B°¨8Ñ$4Ô$4Ò4Ð4Ð4Ð4ØÔ Ò&Ð&Ð&Ð&ØÔ�v r¨2 hÑ/Ô/Ò/Ð/Ð/Ð/ØŒ7•f˜f r§w¢w­q¡z¤zÑ1°2¸¿ºÅ¹
¼
±?Ø  2§7¢7­1¡:¤:™oð/ñ 0ô 0ò 0ð 0ð 0ð 0àÔ˜tœvÒ%Ð%Ð%Ð%ØÔ ¤¨¬Ð 0Ò0Ð0Ð0Ð0ØŒ>×"Ò" 1¤<Ñ0Ô0°A¸¼±GÒ;Ð;Ð;Ð;ØŒ?×#Ò# A¤LÑ1Ô1°Q¸¼±WÒ<Ð<Ð<Ð<ØŒ?×#Ò# B¤NÑ3Ô3°r¸A¼C±xÀ"ÀqÄsÁ(Ñ7JÒJÐJÐJÐJØŒ<× Ò Ø	Œñô Ø˜QœS™ 2¨¬¡8Ñ+¨b°1´3©hÑ6¸¸Q¼S¹Ñ@òAð Að Að AàŒ<×Ò˜AÑÔ 2 #¨¬¡)¨b°1´3©hÑ"6¸À!¹ÀaÄcÑ9IÑ"IÒIÐIÐIÐIØ�<Š<˜‰?Œ?˜f q¤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        t          d¦  «        z  z    	                    ¦   «         k    sJ ‚|j                             |	¦  «        |	j        |	j        t          d¦  «        z  z    	                    ¦   «         k    sJ ‚|j         
                    |j        ¦  «        t          dz  k    sJ ‚|                     |	¦  «                             | d|d|di¦  «        t          d¦  «        k    sJ ‚t          t!          | ¦  «        t          d¦  «         t#          | ¦  «        z  dz  t#          | ¦  «        dz  gt          d¦  «        t#          | ¦  «        z  dz  dt!          | ¦  «        z  d	z  d
z   t          d¦  «        dt!          | ¦  «        z
  z  d	z  gt#          | ¦  «         dz  t          d¦  «        dt!          | ¦  «        z
  z  d	z  t!          | ¦  «        d	z  dz   gg¦  «        }t%          |                     |	¦  «        |z
  ¦  «        t'          d¦  «        k    sJ ‚d S )Nr  zl1:3r8   r@   )ro   rp   rr   ru   rÆ   r   r)   r©   g      Ð?r>   g      è?)r   r   r1   r   r}   r,   r   r"  r‚   Ú	normalizeré   r   r—   r˜   r   r	   r   r   r
   r   )rH   rI   rb   rL   rM   rd   Úl1Úl2r!   r"   r#   r&   r8   r÷   s                 r0   Útest_planar_joint_advancedr,  ³  s�  € õ ,¨LÑ9Ô9Ñ€BˆˆB��B˜Ý�V‰_Œ_�F€BˆÝÑ!Ô!�J€A€qˆ!ˆQÝ�C˜˜A˜r B¨ 8¨R°"°b°Ø!# a¤c¡Ø!#  a¤c¡	Ø&'¤c¨A¬Cµ$°q±'´'©MÑ&9Ø%&¤S¨1¬3µ°a±´©=Ñ%8ð		:ñ 	:ô 	:€Að
 Œ?×"Ò" 1Ñ%Ô%¨!¬#°´µd¸1±g´g±Ñ*=×)HÒ)HÑ)JÔ)JÒJÐJÐJÐJØŒ?×"Ò" 1Ñ%Ô%¨!¬#°´µd¸1±g´g±Ñ*=×)HÒ)HÑ)JÔ)JÒJÐJÐJÐJØŒ?×(Ò(¨¬Ñ-Ô-µ°a±Ò7Ð7Ð7Ð7Ø�5Š5�‰8Œ8×Ò˜b ! R¨¨B°Ð2Ñ3Ô3µs¸1±v´vÒ=Ð=Ð=Ð=ÝÝ	ˆR‰Œ•4˜‘7”7�(�S ™WœWÑ$ qÑ(­#¨b©'¬'°A©+Ð6Ý	ˆa‰Œ•3�r‘7”7Ñ	˜QÑ	 ¥C¨¡G¤G¡¨a¡°%Ñ 7Ý	ˆa‰Œ�A�˜B™œ‘KÑ	  1Ñ	$ð	&å
ˆb‰'Œ'ˆ�A‰•t˜A‘w”w !¥c¨"¡g¤g¡+Ñ.°Ñ2µC¸±G´G¸a±KÀ%Ñ4GÐHð	Jñ Kô K€Eõ �A—E’E˜!‘H”H˜uÑ$Ñ%Ô%­¨q©¬Ò1Ð1Ð1Ð1Ð1Ð1r2   c            	      ó@  — t          ¦   «         \  } }}}t          d¦  «        \  }}}}}}	t          d||¦  «        }
|
j        dk    sJ ‚|
j        |k    sJ ‚|
j        |k    sJ ‚|
j        t          |||g¦  «        k    sJ ‚|
j        t          |||	g¦  «        k    sJ ‚|
j	        t          || 
                    t          ¦  «        z
  || 
                    t          ¦  «        z
  |	| 
                    t          ¦  «        z
  g¦  «        k    sJ ‚|
j                             |j        ¦  «        t          d¦  «        k    sJ ‚|
j                             |j        ¦  «        t          d¦  «        k    sJ ‚|
j                             |
j        ¦  «        t          d¦  «        k    sJ ‚|j                             |j        ¦  «        t          d¦  «        k    sJ ‚|j                             | ¦  «        t          d¦  «        k    sJ ‚|                      |¦  «        | t'          |¦  «        z  t'          |¦  «        z  |t)          |¦  «        z  z
  |j        z  |t)          |¦  «        z  t'          |¦  «        z  |t'          |¦  «        z  z
  |j        z  z   | t)          |¦  «        z  |	z
  |j        z  z   k    sJ ‚|                     | ¦  «        |t'          |¦  «        z  t'          |¦  «        z  |t)          |¦  «        z  z   |j        z  | t)          |¦  «        z  t'          |¦  «        z  |t'          |¦  «        z  z   |j        z  z   |t)          |¦  «        z  |	z   |j        z  z   k    sJ ‚ |
j        ¦   «         dk    sJ ‚|
j        dk    sJ ‚|
j        dk    sJ ‚|
j        �J ‚d S )Núq0:3_S, u0:3_Sr   r   z&SphericalJoint: S  parent: P  child: CÚBODYé{   )r1   r   r   rv   r4   r5   rw   r	   rU   rx   rW   rX   rp   ry   rz   r   ro   r�   r¤   r   r   r+   r,   r}   r|   Ú	_rot_typeÚ
_rot_orderÚ_amounts)r!   r"   r#   r&   rH   rI   rb   rL   rM   rd   r   s              r0   Útest_spherical_jointr4  Î  sm  € ÝÑ!Ô!�J€A€qˆ!ˆQÝ+Ð,<Ñ=Ô=Ñ€BˆˆB��B˜Ý�s˜A˜qÑ!Ô!€AØŒ6�SŠ=ˆ=ˆ=ˆ=ØŒ8�qŠ=ˆ=ˆ=ˆ=ØŒ7�aŠ<ˆ<ˆ<ˆ<ØŒ=�F B¨¨B <Ñ0Ô0Ò0Ð0Ð0Ð0ØŒ8•v˜r 2 r˜lÑ+Ô+Ò+Ð+Ð+Ð+ØŒ6•V˜R "§'¢'­!¡*¤*™_¨b°2·7²7½1±:´:©o¸rÀBÇGÂGÍAÁJÄJ¹ÐOÑPÔPÒPÐPÐPÐPØŒ=×!Ò! !¤,Ñ/Ô/µ6¸!±9´9Ò<Ð<Ð<Ð<ØŒ>×"Ò" 1¤<Ñ0Ô0µF¸1±I´IÒ=Ð=Ð=Ð=ØŒ>×"Ò" 1¤=Ñ1Ô1µV¸A±Y´YÒ>Ð>Ð>Ð>ØŒ<× Ò  ¤Ñ.Ô.µ&¸±)´)Ò;Ð;Ð;Ð;ØŒ<×Ò˜AÑÔ¥&¨¡)¤)Ò+Ð+Ð+Ð+Ø�<Š<˜‰?Œ? ˜s¥S¨¡W¤W™}­s°2©w¬wÑ6¸½cÀ"¹g¼g¹ÑEÈÌÑLØ•�R‘”‰L�3˜r™7œ7Ñ" R­#¨b©'¬'¡\Ñ1°Q´SñP9ñ 9à�3�˜R™œ‘= 2Ñ%¨¬ñ<-ñ-ò -ð -ð -ð -ð �<Š<˜‰?Œ?˜r¥C¨¡G¤G™|­c°"©g¬gÑ5¸½SÀ¹W¼W¹ÑDÈÌÑKØˆC•#�b‘'”'‰M�C ™GœGÑ# b­3¨r©7¬7¡lÑ2°a´cñO:ñ :à�˜B™œ‘< "Ñ$¨¬ñ=,ñ,ò ,ð ,ð ,ð ,ð ˆ1Œ9‰;Œ;ÐBÒBÐBÐBÐBØŒ;˜&Ò Ð Ð Ð ØŒ<˜3ÒÐÐÐØŒ:ÐÐÐÐÐr2   c            	      óÂ  — t          d¦  «        \  } }}t          dd¦  «        \  }}}t          ¦   «         \  }}}}	t          d||	| ||g|||g¬¦  «        }
|
j        t	          | ||g¦  «        k    sJ ‚|
j        t	          |||g¦  «        k    sJ ‚|
j        t	          g d¢¦  «        k    sJ ‚|                     |¦  «        | t          |¦  «        z  t          |¦  «        z  |t          |¦  «        z  z
  |j
        z  |t          |¦  «        z  t          |¦  «        z  |t          |¦  «        z  z
  |j        z  z   | t          |¦  «        z  |z
  |j        z  z   k    sJ ‚d S )Nzq0:3r>   r   )rw   rU   )r   r   r   )r   r1   r   rw   r	   rU   rx   r¤   r   r   r+   r,   r}   )rH   rI   rb   rL   rM   rd   r!   r"   r#   r&   r   s              r0   Ú/test_spherical_joint_speeds_as_derivative_termsr6  é  sv  € õ   Ñ'Ô'�J€BˆˆBÝ ¨Ñ*Ô*�J€BˆˆBÝÑ!Ô!�J€A€qˆ!ˆQÝ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<ÐPÑPÔP€AØŒ=�F B¨¨B <Ñ0Ô0Ò0Ð0Ð0Ð0ØŒ8•v˜r 2 r˜lÑ+Ô+Ò+Ð+Ð+Ð+ØŒ6•V˜I˜I˜IÑ&Ô&Ò&Ð&Ð&Ð&Ø�<Š<˜‰?Œ? ˜s¥S¨¡W¤W™}­s°2©w¬wÑ6¸½cÀ"¹g¼g¹ÑEÈÌÑLØ
�S�‰WŒW‰•s˜2‘w”wÑ ¥c¨"¡g¤g¡Ñ-°´ñP5ñ 5àˆs•S˜‘W”W‰}˜rÑ! Q¤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„¦  «         d S )Nr.  z
q0:4, u0:4r   c            	      ód   •— t          d‰‰ t          ‰‰g¦  «        t          ‰g¦  «        ¦  «        S ©Nr   ©r   r	   )r&   r#   rH   rI   rL   s   €€€€€r0   r:   z-test_spherical_joint_coords.<locals>.<lambda>	  s+   ø€ •> # q¨!­V°R¸°HÑ-=Ô-=½vÀrÀd¹|¼|ÑLÔL€ r2   c                  ól   •	— t          d‰‰ t          ‰‰‰‰g¦  «        t          ‰‰‰g¦  «        ¦  «        S r9  r:  )	r&   r#   rH   rI   rb   rc   rL   rM   rd   s	   €€€€€€€€€r0   r:   z-test_spherical_joint_coords.<locals>.<lambda>  s=   ø€ �~ØˆQ�•6˜2˜r 2 rÐ*Ñ+Ô+­V°R¸¸R°LÑ-AÔ-Añ Cô  C€ r2   c                  ól   •	— t          d‰‰ t          ‰‰‰g¦  «        t          ‰‰‰‰g¦  «        ¦  «        S r9  r:  )	r&   r#   rH   rI   rb   rL   rM   rd   Úu4s	   €€€€€€€€€r0   r:   z-test_spherical_joint_coords.<locals>.<lambda>  s=   ø€ �~ØˆQ�•6˜2˜r 2˜,Ñ'Ô'­°°R¸¸RÐ0@Ñ)AÔ)Añ Cô  C€ r2   )r   r1   r   rw   r	   rU   r   rV   )Úq0sÚq1sÚq2sÚu0sÚu1sÚu2sr!   r"   r   r&   r#   rH   rI   rb   rc   rL   rM   rd   r=  s            @@@@@@@@@@r0   Útest_spherical_joint_coordsrD  ø  sT  øøøøøøøøøø€ Ý#1Ð2BÑ#CÔ#CÑ €Cˆˆc�3˜˜SÝ%3°LÑ%AÔ%AÑ"€BˆˆB��B˜˜B åÑ!Ô!�J€A€qˆ!ˆQÝ�s˜A˜q 2 r¨2 ,°°R¸°Ñ=Ô=€AØŒ=�F B¨¨B <Ñ0Ô0Ò0Ð0Ð0Ð0ØŒ8•v˜r 2 r˜lÑ+Ô+Ò+Ð+Ð+Ð+åÑ!Ô!�J€A€qˆ!ˆQÝ�s˜A˜q¥&¨"¨b°"¨Ñ"6Ô"6Ý˜r 2 r˜lÑ+Ô+ñ	-ô 	-€AàŒ=�F B¨¨B <Ñ0Ô0Ò0Ð0Ð0Ð0ØŒ8•v˜r 2 r˜lÑ+Ô+Ò+Ð+Ð+Ð+åÑ!Ô!�J€A€qˆ!ˆQÝ
�:ØLÐLÐLÐLÐLÐLÐLÐLñNô Nð Nõ �:ð Cð Cð Cð Cð Cð Cð Cð Cð Cð Cð Cð Cñ Dô Dð Då
�:ð Cð Cð Cð Cð Cð Cð Cð Cð Cð Cð Cð Cñ Dô Dð Dð Dð Dr2   c                  óô  ‡‡‡‡‡‡— t          d¦  «        \  ŠŠŠŠŠŠt          t          ‰¦  «         t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  gt          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  z
  t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  z
  gt          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  z
  t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  z   gg¦  «        } t          ‰ t          ‰¦  «        z  ‰z
  g‰ t          ‰¦  «        z  t          ‰¦  «        z  ‰t          ‰¦  «        z  z   g‰t          ‰¦  «        z  t          ‰¦  «        z  ‰t          ‰¦  «        z  z   gg¦  «        }t          t	          d¦  «         ‰t          ‰t
          dz  z   ¦  «        z  t          ‰¦  «        z  ‰t          ‰t
          dz  z   ¦  «        z  z   z  g‰ t          ‰¦  «        z  ‰z
  g‰ t          ‰¦  «        z  ‰z
  gg¦  «        }t          d¦  «        \  }}}}}}t          d||‰‰‰g‰‰‰g|j        |j	        z   |j	         |j
        z   ||dd¬¦  «        }	|	j                             ¦   «         d	k    sJ ‚|	j        dk    sJ ‚t          |                     |¦  «        | z
  ¦  «        t!          d
¦  «        k    sJ ‚t          |                     |¦  «                             |¦  «        |z
  ¦  «        t!          d
d¦  «        k    sJ ‚t          |j                             |¦  «                             |¦  «        ¦  «        |k    sJ ‚t          d¦  «        \  }}}}}}t          d||‰‰‰g‰‰‰g|j        |j	        z   |j	         |j
        z   ||d	‰‰‰fd¬¦  «        }	ˆˆˆˆˆˆfd„}
|	j                             ¦   «         d	k    sJ ‚|	j        dk    sJ ‚t          |                     |¦  «         |
| ¦  «        z
  ¦  «        t!          d
¦  «        k    sJ ‚t          |                     |¦  «                             |¦  «         |
|¦  «        z
  ¦  «        t!          d
d¦  «        k    sJ ‚t          |j                             |¦  «                             |¦  «        ¦  «         |
|¦  «        k    sJ ‚t          d¦  «        \  }}}}}}t          d||‰‰‰g‰‰‰g|j        |j	        z   |j	         |j
        z   ||dd¬¦  «        }	|	j                             ¦   «         d	k    sJ ‚|	j        dk    sJ ‚t          |                     |¦  «        t          t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  z
  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  z   gt          ‰¦  «         t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «         t          ‰¦  «        z  gt          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  z
  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  z
  gg¦  «        z
  ¦  «        t!          d
¦  «        k    sJ ‚t          |                     |¦  «                             |¦  «        t          ‰t          ‰¦  «        z  ‰z
  g‰t          ‰¦  «        z  t          ‰¦  «        z  ‰t          ‰¦  «        z  z
  g‰t          ‰¦  «        z  t          ‰¦  «        z  ‰t          ‰¦  «        z  z   gg¦  «        z
  ¦  «        t!          d
d¦  «        k    sJ ‚t          |j                             |¦  «                             |¦  «        ¦  «        t          t	          d¦  «         ‰t          ‰t
          dz  z   ¦  «        z  t          ‰¦  «        z  ‰t          ‰t
          dz  z   ¦  «        z  z   z  g‰t          ‰¦  «        z  ‰z
  g‰t          ‰¦  «        z  ‰z
  gg¦  «        k    sJ ‚d S )Nr  r)   r©   Tr   Úbodyr0  ©rw   rU   ro   rp   rr   ru   Úrot_typeÚ	rot_orderr/  r@   r>   ©	rw   rU   ro   rp   rr   ru   rH  ÚamountsrI  c                 óF   •— |                       ‰‰‰‰‰‰‰‰‰‰‰‰i¦  «        S rñ   ©r˜   ©ÚexprrH   rI   rb   rL   rM   rd   s    €€€€€€r0   r:   z2test_spherical_joint_orient_body.<locals>.<lambda>0  ó1   ø€  §¢Ø	ˆR��R˜˜R  R¨¨R°°RÐ8ñ!:ô !:€ r2   ÚBodYÚyxz)r   r	   r   r   r   r   r1   r   r+   r,   r}   r1  Úupperr2  r
   r—   r   r¤   rÿ   rz   r�   ©r÷   ÚN_w_AÚN_v_Cor!   r"   r#   r&   r.   r/   r   Úswitch_orderrH   rI   rb   rL   rM   rd   s              @@@@@@r0   Ú test_spherical_joint_orient_bodyrX    s/	  øøøøøø€ Ý+¨LÑ9Ô9Ñ€BˆˆB��B˜ÝÝ
ˆb‰'Œ'ˆ•C˜‘G”G�8�c "™gœgÑ%¥s¨2¡w¤wµ°R±´Ñ'8Ð9Ý
ˆb‰'Œ'ˆ•C˜‘G”GÑ	�S ™WœW¥s¨2¡w¤wÑ.µ°R±´Ñ8½3¸r¹7¼7ÅSÈÁWÄWÑ;LÑLÝ
ˆb‰'Œ'ˆ•C˜‘G”GÑ	�c "™gœgÑ	%­¨B©¬µ#°b±'´'Ñ(9Ñ	9ð	;å	ˆR‰Œ•3�r‘7”7Ñ	�S ™WœW˜H¥s¨2¡w¤wÑ.µ°R±´½3¸r¹7¼7Ñ1BÅSÈÁWÄWÑ1LÑLÝ
ˆb‰'Œ'ˆ•C˜‘G”GÑ	�c "™gœg­¨B©¬Ñ/µ#°b±'´'Ñ9Ñ	9ð	;ð	<ñ =ô =€Eõ �b�S�3˜r™7œ7‘] RÑ'Ð(Ø�S�3˜r™7œ7‘]¥S¨¡W¤WÑ,¨rµC¸±G´G©|Ñ;Ð<Ø�#˜b™'œ'‘\¥C¨¡G¤GÑ+¨bµ3°r±7´7©lÑ:Ð;ð=ñ >ô >€Eõ Ý
ˆq‰'Œ'ˆ�R�#˜b¥2¨¡6™kÑ*Ô*Ñ*­S°©W¬WÑ4°r½CÀÅRÈ!ÁVÁÑ<LÔ<LÑ7LÑLÑ	MÐNØ
ˆ�s�2‰wŒw‰˜Ñ	Ð ˜s¥S¨¡W¤W™}¨rÑ1Ð2ð4ñ 5ô 5€Fõ ,¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<Ø$%¤C¨!¬#¡I¸A¼C¸4À!Ä#¹:Ø)-ÀØ &°#ð	7ñ 	7ô 	7€Að Œ;×ÒÑÔ &Ò(Ð(Ð(Ð(ØŒ<˜3ÒÐÐÐÝ�A—E’E˜!‘H”H˜uÑ$Ñ%Ô%­¨q©¬Ò1Ð1Ð1Ð1Ý�A—L’L ‘O”O×-Ò-¨aÑ0Ô0°5Ñ8Ñ9Ô9½UÀ1Àa¹[¼[ÒHÐHÐHÐHÝ�A”L×$Ò$ QÑ'Ô'×1Ò1°!Ñ4Ô4Ñ5Ô5¸Ò?Ð?Ð?Ð?å+¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<Ø$%¤C¨!¬#¡I¸A¼C¸4À!Ä#¹:Ø)-ÀØ &°°R¸°Èð	Mñ 	Mô 	M€Að:ð :ð :ð :ð :ð :ð :ð :ð :€LàŒ;×ÒÑÔ &Ò(Ð(Ð(Ð(ØŒ<˜3ÒÐÐÐÝ�A—E’E˜!‘H”H˜|˜|¨EÑ2Ô2Ñ2Ñ3Ô3µu¸Q±x´xÒ?Ð?Ð?Ð?Ý�A—L’L ‘O”O×-Ò-¨aÑ0Ô0°<°<ÀÑ3FÔ3FÑFñ ô Ý!& q¨!¡¤ò-ð -ð -ð -å�A”L×$Ò$ QÑ'Ô'×1Ò1°!Ñ4Ô4Ñ5Ô5¸¸ÀfÑ9MÔ9MÒMÐMÐMÐMå+¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<Ø$%¤C¨!¬#¡I¸A¼C¸4À!Ä#¹:Ø)-ÀØ &°%ð	9ñ 	9ô 	9€Að Œ;×ÒÑÔ &Ò(Ð(Ð(Ð(ØŒ<˜5Ò Ð Ð Ð Ý�A—E’E˜!‘H”H�vÝ
ˆb‰'Œ'ˆ•C˜‘G”GÑ	�S ™WœW¥s¨2¡w¤wÑ.µ°R±´Ñ8½3¸r¹7¼7ÅSÈÁWÄWÑ;LÑLÝ	ˆR‰Œ•3�r‘7”7Ñ	�S ™WœWÑ	$¥s¨2¡w¤wµ°R±´Ñ'8Ñ	8ð	:å
ˆb‰'Œ'ˆ•C˜‘G”G�8�c "™gœgÑ%­¨B©¬ xµ#°b±'´'Ñ'9Ð:Ý	ˆR‰Œ•3�r‘7”7Ñ	�S ™WœW˜H¥s¨2¡w¤wÑ.µ°R±´½3¸r¹7¼7Ñ1BÅSÈÁWÄWÑ1LÑLÝ	ˆR‰Œ•3�r‘7”7Ñ	�S ™WœW¥s¨2¡w¤wÑ.µ°R±´Ñ8Ñ	8ð	:ð	';ñ  <ô  <ñ <ñ =ô =õ
 AFÀaÁÄòIð Ið Ið Iõ �A—L’L ‘O”O×-Ò-¨aÑ0Ô0µ6Ø	�c�"‰gŒg‰˜Ñ	Ð˜b¥3 r¡7¤7™l­S°©W¬WÑ4°r½CÀ¹G¼G±|ÑCÐDØ	�c�"‰gŒg‰�˜B™œÑ	 "¥s¨2¡w¤w¡,Ñ	.Ð/ð;1ñ 42ô 42ñ 2ñ 3ô 3å6;¸A¸q±k´kòBð Bð Bð Bõ �A”L×$Ò$ QÑ'Ô'×1Ò1°!Ñ4Ô4Ñ5Ô5½Ý
ˆq‰'Œ'ˆ�R�#˜b¥2¨¡6™kÑ*Ô*Ñ*­S°©W¬WÑ4°r½CÀÅRÈ!ÁVÁÑ<LÔ<LÑ7LÑLÑ	MÐNØ	�c�"‰gŒg‰˜Ñ	Ð˜b¥3 r¡7¤7™l¨RÑ/Ð0ðA2ñ :3ô :3ò 3ð 3ð 3ð 3ð 3ð 3r2   c                  ó  ‡‡‡‡‡‡— t          d¦  «        \  ŠŠŠŠŠŠt          t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  z
  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  z
  t          ‰¦  «        t          ‰¦  «        z  gt          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  z   t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  z
  t          ‰¦  «         t          ‰¦  «        z  gt          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        gg¦  «        } t          ‰t          ‰¦  «        z  ‰t          ‰¦  «        z  t          ‰¦  «        z  z
  g‰t          ‰¦  «        z  ‰t          ‰¦  «        z  t          ‰¦  «        z  z   g‰‰t          ‰¦  «        z  z
  gg¦  «        }t          ‰‰t          ‰¦  «        z  z
  g‰‰t          ‰¦  «        z  z
  gt	          d¦  «        ‰ t          ‰t
          dz  z   ¦  «        z  ‰t          ‰t
          dz  z   ¦  «        z  t          ‰¦  «        z  z   z  gg¦  «        }t          d¦  «        \  }}}}}}t          d||‰‰‰g‰‰‰g|j        |j	        z   |j         |j
        z   ||dd¬¦  «        }	|	j                             ¦   «         d	k    sJ ‚|	j        dk    sJ ‚t          |                     |¦  «        | z
  ¦  «        t!          d
¦  «        k    sJ ‚t          |                     |¦  «                             |¦  «        ¦  «        |k    sJ ‚t          |j                             |¦  «                             |¦  «        ¦  «        |k    sJ ‚ˆˆˆˆˆˆfd„}
t          d¦  «        \  }}}}}}t          d||‰‰‰g‰‰‰g|j        |j	        z   |j         |j
        z   ||d	‰‰‰fd¬¦  «        }	|	j                             ¦   «         d	k    sJ ‚|	j        dk    sJ ‚t          |                     |¦  «         |
| ¦  «        z
  ¦  «        t!          d
¦  «        k    sJ ‚t          |                     |¦  «                             |¦  «        ¦  «         |
|¦  «        k    sJ ‚t          |j                             |¦  «                             |¦  «        ¦  «         |
|¦  «        k    sJ ‚t          d¦  «        \  }}}}}}t          d||‰‰‰g‰‰‰g|j        |j	        z   |j         |j
        z   ||dd¬¦  «        }	|	j                             ¦   «         d	k    sJ ‚|	j        dk    sJ ‚t          |                     |¦  «        t          t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  z   t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  z   gt          ‰¦  «         t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «         t          ‰¦  «        z  gt          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  t          ‰¦  «        z  z
  t          ‰¦  «         t          ‰¦  «        z  t          ‰¦  «        z  t          ‰¦  «        t          ‰¦  «        z  z   gg¦  «        z
  ¦  «        sJ ‚t          |                     |¦  «                             |¦  «        t          ‰ ‰t          ‰¦  «        z  z   g‰ t          ‰¦  «        z  ‰t          ‰¦  «        z  t          ‰¦  «        z  z   g‰t          ‰¦  «        z  ‰t          ‰¦  «        z  t          ‰¦  «        z  z   gg¦  «        z
  ¦  «        t!          d
d¦  «        k    sJ ‚t          |j                             |¦  «                             |¦  «        t          ‰t          ‰¦  «        z  ‰t          ‰¦  «        z  t          ‰¦  «        z  z   g‰t          ‰¦  «        z  ‰t          ‰¦  «        z  t          ‰¦  «        z  z   g‰‰t          ‰¦  «        z  z   ‰t          ‰¦  «        z  z
  ‰t          ‰¦  «        z  t          ‰¦  «        z  z
  gg¦  «        z
  ¦  «        t!          d
d¦  «        k    sJ ‚d S )Nr  r)   r©   Tr   Úspacer0  rG  ÚSPACEr@   c                 óF   •— |                       ‰‰‰‰‰‰‰‰‰‰‰‰i¦  «        S rñ   rM  rN  s    €€€€€€r0   r:   z3test_spherical_joint_orient_space.<locals>.<lambda>h  rP  r2   rJ  ÚSPaCeÚzxyr>   )r   r	   r   r   r   r   r1   r   r+   r}   r,   r1  rS  r2  r
   r—   r   r¤   rÿ   rz   r�   rT  s              @@@@@@r0   Ú!test_spherical_joint_orient_spacer_  N  s0	  øøøøøø€ Ý+¨LÑ9Ô9Ñ€BˆˆB��B˜ÝÝ
ˆb‰'Œ'ˆ•C˜‘G”GÑ	�c "™gœg­¨B©¬Ñ/µ#°b±'´'Ñ9Ñ	9Ý	ˆR‰Œ•3�r‘7”7Ñ	�S ™WœWÑ	$¥s¨2¡w¤wµ°R±´Ñ'8Ñ	8½#¸b¹'¼'ÅCÈÁGÄGÑ:Kð	Må
ˆb‰'Œ'ˆ•C˜‘G”GÑ	�c "™gœg­¨B©¬Ñ/µ#°b±'´'Ñ9Ñ	9Ý
ˆb‰'Œ'ˆ•C˜‘G”GÑ	�c "™gœgÑ	%­¨B©¬µ#°b±'´'Ñ(9Ñ	9½CÀ¹G¼G¸8ÅcÈ"ÁgÄgÑ;Mð	Oå	ˆR‰Œ•3�r‘7”7Ñ	�S ™WœW˜H¥s¨2¡w¤wÑ.µ°B±´Ð8ð:ñ ;ô ;€Eõ Ø	�c�"‰gŒg‰˜�S ™WœW™¥s¨2¡w¤wÑ.Ñ	.Ð/Ø	�c�"‰gŒg‰˜�S ™WœW™¥s¨2¡w¤wÑ.Ñ	.Ð/°"°r½CÀ¹G¼G±|Ñ2CÐ1DðFñ Gô G€Eõ Ø	ˆb•3�r‘7”7‰lÑ	Ð˜b 2­¨B©¬¡<Ñ/Ð0Ý	ˆa‰Œ�R�C�#˜b¥2¨¡6™kÑ*Ô*Ñ*¨Rµ#°b½2À¹6±kÑ2BÔ2BÑ-BÅSÈÁWÄWÑ-LÑLÑ	MÐNðPñ Qô Q€Fõ ,¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<Ø$%¤C¨!¬#¡I¸A¼C¸4À!Ä#¹:Ø)-ÀØ '°3ð	8ñ 	8ô 	8€Að Œ;×ÒÑÔ 'Ò)Ð)Ð)Ð)ØŒ<˜3ÒÐÐÐÝ�A—E’E˜!‘H”H˜uÑ$Ñ%Ô%­¨q©¬Ò1Ð1Ð1Ð1Ý�A—L’L ‘O”O×-Ò-¨aÑ0Ô0Ñ1Ô1°UÒ:Ð:Ð:Ð:Ý�A”L×$Ò$ QÑ'Ô'×1Ò1°!Ñ4Ô4Ñ5Ô5¸Ò?Ð?Ð?Ð?ð:ð :ð :ð :ð :ð :ð :ð :ð :€Lå+¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<Ø$%¤C¨!¬#¡I¸A¼C¸4À!Ä#¹:Ø)-ÀØ '°"°b¸"°Èð	Nñ 	Nô 	N€Að Œ;×ÒÑÔ 'Ò)Ð)Ð)Ð)ØŒ<˜3ÒÐÐÐÝ�A—E’E˜!‘H”H˜|˜|¨EÑ2Ô2Ñ2Ñ3Ô3µu¸Q±x´xÒ?Ð?Ð?Ð?Ý�A—L’L ‘O”O×-Ò-¨aÑ0Ô0Ñ1Ô1°\°\À%Ñ5HÔ5HÒHÐHÐHÐHÝ�A”L×$Ò$ QÑ'Ô'×1Ò1°!Ñ4Ô4Ñ5Ô5¸¸ÀfÑ9MÔ9MÒMÐMÐMÐMå+¨DÑ1Ô1Ñ€A€qˆ!ˆQ��dÝ�s˜A˜q¨r°2°r¨lÀBÈÈBÀ<Ø$%¤C¨!¬#¡I¸A¼C¸4À!Ä#¹:Ø)-ÀØ '°5ð	:ñ 	:ô 	:€Að Œ;×ÒÑÔ 'Ò)Ð)Ð)Ð)ØŒ<˜5Ò Ð Ð Ð Ý�A—E’E˜!‘H”H�vÝ
ˆb‰'Œ'ˆ•C˜‘G”GÑ	�c "™gœg˜X­¨B©¬Ñ/µ#°b±'´'½CÀ¹G¼GÑ2CÅcÈ"ÁgÄgÑ2MÑMÝ	ˆR‰Œ•3�r‘7”7Ñ	�S ™WœWÑ	$¥s¨2¡w¤wµ°R±´Ñ'8Ñ	8ð	:å
ˆb‰'Œ'ˆ•C˜‘G”G�8�c "™gœgÑ%­¨B©¬ xµ#°b±'´'Ñ'9Ð:Ý	ˆR‰Œ•3�r‘7”7Ñ	�S ™WœW˜H¥s¨2¡w¤wÑ.µ°R±´½3¸r¹7¼7Ñ1BÅSÈÁWÄWÑ1LÑLÝ
ˆb‰'Œ'ˆ•C˜‘G”GÑ	�c "™gœgÑ	%­¨B©¬µ#°b±'´'Ñ(9Ñ	9ð	;ð	'<ñ  =ô  =ñ =ñ >ô >ð >ð >ð >õ �A—L’L ‘O”O×-Ò-¨aÑ0Ô0µ6Ø
ˆˆr•C˜‘G”G‰|Ñ	Ð ˜s¥S¨¡W¤W™}¨rµC¸±G´G©|½cÀ"¹g¼gÑ/EÑEÐFØ	�c�"‰gŒg‰˜�S ™WœW™¥s¨2¡w¤wÑ.Ñ	.Ð/ð;1ñ 42ô 42ñ 2ñ 3ô 3å6;¸A¸q±k´kòBð Bð Bð Bõ �A”L×$Ò$ QÑ'Ô'×1Ò1°!Ñ4Ô4µvØ	�c�"‰gŒg‰˜�S ™WœW™¥s¨2¡w¤wÑ.Ñ	.Ð/Ø	�c�"‰gŒg‰˜�S ™WœW™¥s¨2¡w¤wÑ.Ñ	.Ð/Ø	ˆb•3�r‘7”7‰lÑ	˜R¥# b¡'¤'™\Ñ	)Ø	�c�"‰gŒg‰�˜B™œÑ	ñ
 ð 	!ð?"ñ 8#ô 8#ñ #ñ $ô $õ (-¨Q°¡{¤{ò	3ð 3ð 3ð 3ð 3ð 3r2   c                  óü  — t          ¦   «         \  } } }}t          d||¦  «        }|j        dk    sJ ‚|j        |k    sJ ‚|j        |k    sJ ‚|j        t          ¦   «         k    sJ ‚|j        t          ¦   «         k    sJ ‚|j        t          ddg ¦  «        j	        k    sJ ‚|j
                             |j
        ¦  «        t          d¦  «        k    sJ ‚|j                             |j        ¦  «        t!          d¦  «        k    sJ ‚|j                             |j        ¦  «        t!          d¦  «        k    sJ ‚|j                             |j        ¦  «        t!          d¦  «        k    sJ ‚|j                             |j        ¦  «        t!          d¦  «        k    sJ ‚|j                             |j
        ¦  «        t!          d¦  «        k    sJ ‚|j
                             |j
        ¦  «        dk    sJ ‚|j
                             |j
        ¦  «        dk    sJ ‚|                     ¦   «         dk    sJ ‚t          ¦   «         \  }}}}t+          d¦  «        \  }}t-          d¦  «        }|                     |j
        |j        t2          dz  ¦  «         t          d||||j
        j        z  ||j
        j        z  |¬	¦  «        }|j                             |j        ¦  «        ||j
        j        z  k    sJ ‚|j                             |j        ¦  «        ||j
        j        z  k    sJ ‚|j                             |j        ¦  «        t!          d¦  «        k    sJ ‚|j                             |j        ¦  «        | |j        z  ||j        z  z   k    sJ ‚|j                             |j
        ¦  «        t!          d¦  «        k    sJ ‚|j                             |¦  «        t!          d¦  «        k    sJ ‚|j
                             |j
        ¦  «        dk    sJ ‚|j
                             |j
        ¦  «        dk    sJ ‚|j        |j        k    sJ ‚t9          ¦   «         5  t;          ||¦  «         d d d ¦  «         d S # 1 swxY w Y   d S )
NÚWr>   r   r@   z!WeldJoint: W  parent: P  child: Cri   r'   r)   )ro   rp   rr   )r1   r   rv   r4   r5   rw   r	   rU   rx   ÚTr%   r—   r   rp   ry   rz   r   ro   r�   r¤   r|   r   r   r*   r,   r   r+   r}   r   r   )	rf   r#   r&   ra  r!   r"   rƒ   r„   r.   s	            r0   Útest_weld_jointrc  Œ  sX  € ÝÑ!Ô!�J€A€qˆ!ˆQÝ�#�q˜!ÑÔ€AØŒ6�SŠ=ˆ=ˆ=ˆ=ØŒ8�qŠ=ˆ=ˆ=ˆ=ØŒ7�aŠ<ˆ<ˆ<ˆ<ØŒ=�F™HœHÒ$Ð$Ð$Ð$ØŒ8•v‘x”xÒÐÐÐØŒ6•V˜A˜q "Ñ%Ô%Ô'Ò'Ð'Ð'Ð'ØŒ7�;Š;�q”wÑÔ¥3 q¡6¤6Ò)Ð)Ð)Ð)ØŒ=×!Ò! !¤,Ñ/Ô/µ6¸!±9´9Ò<Ð<Ð<Ð<ØŒ>×"Ò" 1¤<Ñ0Ô0µF¸1±I´IÒ=Ð=Ð=Ð=ØŒ>×"Ò" 1¤=Ñ1Ô1µV¸A±Y´YÒ>Ð>Ð>Ð>ØŒ<× Ò  ¤Ñ.Ô.µ&¸±)´)Ò;Ð;Ð;Ð;ØŒ<×Ò˜AœGÑ$Ô$­¨q©	¬	Ò1Ð1Ð1Ð1ØŒ7×Ò˜aœgÑ&Ô&¨!Ò+Ð+Ð+Ð+ØŒ7×Ò˜aœgÑ&Ô&¨!Ò+Ð+Ð+Ð+Ø�9Š9‰;Œ;Ð=Ò=Ð=Ð=Ð=åÑ!Ô!�J€A€qˆ!ˆQÝ�5‰>Œ>�D€A€qÝ˜'Ñ"Ô"€DØ×Ò�Q”W˜aœc¥2¨¡6Ñ*Ô*Ð*Ý�#�q˜!¨!¨a¬g¬i©-Ø !¤'¤)™m¸tð	Eñ 	Eô 	E€Að Œ=×!Ò! !¤,Ñ/Ô/°1°q´w´y±=Ò@Ð@Ð@Ð@ØŒ>×"Ò" 1¤<Ñ0Ô0°A¸¼¼	±MÒAÐAÐAÐAØŒ>×"Ò" 1¤=Ñ1Ô1µV¸A±Y´YÒ>Ð>Ð>Ð>ØŒ<× Ò  ¤Ñ.Ô.°A°#¸¼±)¸aÀ!Ä#¹gÑ2EÒEÐEÐEÐEØŒ<×Ò˜AœGÑ$Ô$­¨q©	¬	Ò1Ð1Ð1Ð1ØŒ<×Ò˜DÑ!Ô!¥V¨A¡Y¤YÒ.Ð.Ð.Ð.ØŒ7×Ò˜aœgÑ&Ô&¨!Ò+Ð+Ð+Ð+ØŒ7×Ò˜aœgÑ&Ô&¨!Ò+Ð+Ð+Ð+ØŒ3�!”#Š:ˆ:ˆ:ˆ:å	Ñ	!Ô	!ð ð Ý�Q˜ÑÔÐðð ð ñ ô ð ð ð ð ð ð ð øøøð ð ð ð ð ð s   ÑQ1Ñ1Q5Ñ8Q5c            
      ó¸  — t          d¦  «        \  } }t          ¦   «         \  }}}}t          ¦   «         5  t          d|||j         ¬¦  «         d d d ¦  «         n# 1 swxY w Y   |j                              |j        ¦  «        dk    sJ ‚|j                             |¦  «         |j        k    sJ ‚|                     |¦  «        t          g d¢dt          | ¦  «         t          | ¦  «         gdt          | ¦  «         t          | ¦  «        gg¦  «        k    sJ ‚|                     |¦  «        ||j        z  k    sJ ‚|                     |¦  «                             ¦   «         t          |dz  ¦  «        k    sJ ‚t          ¦   «         \  }}}}t          ¦   «         5  t          d|||j        |j        z   ¬¦  «         d d d ¦  «         n# 1 swxY w Y   |j                             |j        |j        z   ¦  «        dk    sJ ‚|j                             |¦  «        |j        |j        z   t          d¦  «        z  k    sJ ‚|                     |¦  «        t          t          d¦  «        dz  t          d¦  «        dz  dgt          d¦  «         dz  t          d¦  «        dz  dgg d¢g¦  «        k    sJ ‚|                     |¦  «        t!          d¦  «        k    sJ ‚d S )	Nrj   r8   )Ú
child_axisr   rÏ   r)   )Úparent_axisr�   )r   r1   r   r   r+   ré   r‚   r—   r	   r   r   r¤   rê   r   r   r,   r   )rC   rA   r!   r"   r#   r&   s         r0   Ú!test_deprecated_parent_child_axisrg  ´  s5  € Ý˜*Ñ%Ô%�D€A€qÝÑ!Ô!�J€A€qˆ!ˆQÝ	Ñ	!Ô	!ð -ð -Ý��a˜¨¬ tÐ,Ñ,Ô,Ð,ð-ð -ð -ñ -ô -ð -ð -ð -ð -ð -ð -øøøð -ð -ð -ð -àŒSˆD×Ò ¤Ñ$Ô$¨Ò)Ð)Ð)Ð)ØŒC�KŠK˜‰NŒNˆ?˜aœcÒ!Ð!Ð!Ð!Ø�5Š5�‰8Œ8•v˜z˜z˜zØ !¥C¨¡F¤F 7­S°©V¬V¨GÐ4Ø !¥C¨¡F¤F 7­C°©F¬FÐ3ð5ñ 6ô 6ò 6ð 6ð 6ð 6ð �<Š<˜‰?Œ?˜a !¤#™gÒ%Ð%Ð%Ð%Ø�<Š<˜‰?Œ?×$Ò$Ñ&Ô&­$¨q°A©v©,¬,Ò6Ð6Ð6Ð6åÑ!Ô!�J€A€qˆ!ˆQÝ	Ñ	!Ô	!ð 9ð 9Ý�s˜A˜q¨a¬c°A´C©iÐ8Ñ8Ô8Ð8ð9ð 9ð 9ñ 9ô 9ð 9ð 9ð 9ð 9ð 9ð 9øøøð 9ð 9ð 9ð 9àŒC×Ò˜qœs Q¤S™yÑ)Ô)¨QÒ.Ð.Ð.Ð.ØŒ3�;Š;�q‰>Œ>˜aœc A¤C™i­4°©7¬7Ñ2Ò2Ð2Ð2Ð2Ø�5Š5�‰8Œ8•v¥ Q¡¤¨!¡­T°!©W¬W°q©[¸!Ð<Ý!% a¡¤ ¨1¡­d°1©g¬g¸©k¸1Ð=¸y¸y¸yðJñ Kô Kò Kð Kð Kð Kà�<Š<˜‰?Œ?�f Q™iœiÒ'Ð'Ð'Ð'Ð'Ð's#   ´AÁAÁ!AÆ!G	Ç	GÇGc            	      ó2  — t          ¦   «         \  } }}}t          ¦   «         5  t          d||| j        | j        z   |j        |j        z
  ¬¦  «        }d d d ¦  «         n# 1 swxY w Y   |j                             |j        ¦  «        | j        | j        z   k    sJ ‚|j	                             |j        ¦  «        |j        |j        z
  k    sJ ‚t          ¦   «         \  } }}}t          ¦   «         5  t          d||| j        | j        z   |j        |j        z
  ¬¦  «        }d d d ¦  «         n# 1 swxY w Y   |j                             |j        ¦  «        | j        | j        z   k    sJ ‚|j	                             |j        ¦  «        |j        |j        z
  k    sJ ‚d S )Nr8   )Úparent_joint_posÚchild_joint_pos)r1   r   r   r+   r,   r}   ro   ry   rz   rp   r   )r!   r"   r#   r&   rß   Úsliders         r0   Útest_deprecated_joint_posrl  Ë  s  € ÝÑ!Ô!�J€A€qˆ!ˆQÝ	Ñ	!Ô	!ð 2ð 2Ý�s˜A˜q°1´3¸¼±9Ø'(¤s¨Q¬S¡yð2ñ 2ô 2ˆð2ð 2ð 2ñ 2ô 2ð 2ð 2ð 2ð 2ð 2ð 2øøøð 2ð 2ð 2ð 2ð Ô×$Ò$ Q¤\Ñ2Ô2°a´c¸A¼C±iÒ?Ð?Ð?Ð?ØŒ?×#Ò# A¤LÑ1Ô1°Q´S¸1¼3±YÒ>Ð>Ð>Ð>åÑ!Ô!�J€A€qˆ!ˆQÝ	Ñ	!Ô	!ð ;ð ;Ý  Q¨¸A¼CÀ!Ä#¹IØ01´°a´c±	ð;ñ ;ô ;ˆð;ð ;ð ;ñ ;ô ;ð ;ð ;ð ;ð ;ð ;ð ;øøøð ;ð ;ð ;ð ;ð Ô×'Ò'¨¬Ñ5Ô5¸¼¸q¼s¹ÒBÐBÐBÐBØÔ×&Ò& q¤|Ñ4Ô4¸¼¸a¼c¹	ÒAÐAÐAÐAÐAÐAs#   ¢/AÁA!Á$A!Ã-/D(Ä(D,Ä/D,N)F)EÚsympy.core.functionr   Úsympy.core.numbersr   Úsympy.core.singletonr   Ú(sympy.functions.elementary.miscellaneousr   Ú(sympy.functions.elementary.trigonometricr   r   Úsympyr	   r
   r   r   Úsympy.core.symbolr   Úsympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   Úsympy.physics.mechanics.jointr   Úsympy.physics.vectorr   r   r   Úsympy.testing.pytestr   r   Ú_trX   r1   r<   rg   r†   rš   rž   r§   r¶   rÊ   rà   rï   rô   rø   r  r  r  r  r  r'  r,  r4  r6  rD  rX  r_  rc  rg  rl  r�   r2   r0   ú<module>ry     ss  ðØ *Ð *Ð *Ð *Ð *Ð *Ø !Ð !Ð !Ð !Ð !Ð !Ø "Ð "Ð "Ð "Ð "Ð "Ø 9Ð 9Ð 9Ð 9Ð 9Ð 9Ø ?Ð ?Ð ?Ð ?Ð ?Ð ?Ð ?Ð ?Ø .Ð .Ð .Ð .Ð .Ð .Ð .Ð .Ð .Ð .Ð .Ð .Ø %Ð %Ð %Ð %Ð %Ð %ðDð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð Dð 0Ð /Ð /Ð /Ð /Ð /Ø >Ð >Ð >Ð >Ð >Ð >Ð >Ð >Ð >Ð >Ø ?Ð ?Ð ?Ð ?Ð ?Ð ?Ð ?Ð ?ð Ô€ð
ð 
ð 
ð 
ð9ð 9ð 9ð,Oð ,Oð ,Oð^6&ð 6&ð 6&ðrF6ð F6ð F6ðR5ð 5ð 5ð"%Að %Að %AðP,?ð ,?ð ,?ð^I/ð I/ð I/ðX5"ð 5"ð 5"ðpLDð LDð LDð^"ð "ð "ð$Hð Hð Hð.Mð Mð Mð<Kð Kð Kð03,ð 3,ð 3,ðla ð a ð a ðH.'ð .'ð .'ðb4+ð 4+ð 4+ðn2ð 2ð 2ð6ð ð ð6)ð )ð )ðDð Dð Dð2:3ð :3ð :3ðz;3ð ;3ð ;3ð|%ð %ð %ðP(ð (ð (ð.Bð Bð Bð Bð Br2   