§
    OŠtj#/  ã                   óÈ   — d dl mZmZmZmZmZmZmZ d dlm	Z	m
Z
mZmZ d dlmZmZ d dlmZmZ d„ Zd„ Zd„ Zd„ Zd	„ Zd
„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Z d„ Z!d„ Z"d„ Z#dS )é    )ÚSymbolÚsymbolsÚsinÚcosÚMatrixÚzerosÚsimplify)ÚPointÚReferenceFrameÚdynamicsymbolsÚDyadic)ÚinertiaÚBody)ÚraisesÚwarns_deprecated_sympyc            
      óH  — t          ¦   «         5  t          d¦  «        } d d d ¦  «         n# 1 swxY w Y   | j        dk    sJ ‚| j        g k    sJ ‚t	          d¦  «        }|                     | j        d¦  «         | j        }| j        }|                     |¦  «        |                     |¦  «        k    sJ ‚| j	        t          d¦  «        k    sJ ‚t          d¦  «        \  }}}t          d¦  «        \  }}}	| j        t          | j        ||||||	¦  «        | j        fk    sJ ‚d S )NÚbodyÚbody_masscenterr   Ú	body_masszbody_ixx body_iyy body_izzzbody_ixy body_iyz body_izx)r   r   ÚnameÚloadsr
   Úset_velÚframeÚ
masscenterÚvelÚmassr   r   r   )
r   ÚpointÚcomr   ÚixxÚiyyÚizzÚixyÚiyzÚizxs
             úe/var/www/html/CA-Chatbot/venv/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_body.pyÚtest_defaultr&      su  € Ý	Ñ	!Ô	!ð ð Ý�F‰|Œ|ˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð àŒ9˜ÒÐÐÐØŒ:˜ÒÐÐÐÝÐ#Ñ$Ô$€EØ	‡M‚M�$”*˜aÑ Ô Ð Ø
Œ/€CØŒJ€EØ�7Š7�5‰>Œ>˜UŸYšY uÑ-Ô-Ò-Ð-Ð-Ð-ØŒ9�˜{Ñ+Ô+Ò+Ð+Ð+Ð+ÝÐ8Ñ9Ô9�M€CˆˆcÝÐ8Ñ9Ô9�M€CˆˆcØŒ<�G D¤J°°S¸#¸sÀCÈÑMÔMØ œOð-ò -ð -ð -ð -ð -ð -s   �+«/²/c                  óÐ  — t          d¦  «        } t          d¦  «        }t          d¦  «        }t          |ddd¦  «        }t	          ¦   «         5  t          d| |||¦  «        }d d d ¦  «         n# 1 swxY w Y   |j        }|j        }|                      |d¦  «         | 	                    |¦  «        |  	                    |¦  «        k    sJ ‚| 
                    |¦  «        |  
                    |¦  «        k    sJ ‚|j        |k    sJ ‚|j        || fk    sJ ‚|j        sJ ‚t          |d¦  «        sJ ‚t          |d¦  «        sJ ‚t          |d	¦  «        sJ ‚t          |d
¦  «        sJ ‚d S )NÚrigidbody_masscenterÚrigidbody_massÚrigidbody_frameé   r   Úrigidbody_bodyr   r   r   r   )r
   r   r   r   r   r   r   r   r   r   Úpos_fromr   Úis_rigidbodyÚhasattr)r(   r)   r*   Úbody_inertiaÚ
rigid_bodyr   r   s          r%   Útest_custom_rigid_bodyr2      sè  € å Ð!7Ñ8Ô8ÐÝÐ,Ñ-Ô-€NÝ$Ð%6Ñ7Ô7€OÝ˜?¨A¨q°!Ñ4Ô4€LÝ	Ñ	!Ô	!ð Ið IÝÐ*Ð,@Ø(¨/¸<ñIô Iˆ
ðIð Ið Iñ Iô Ið Ið Ið Ið Ið Ið Iøøøð Ið Ið Ið Ið Ô
€CØÔ€EØ× Ò  °!Ñ4Ô4Ð4Ø�7Š7�5‰>Œ>Ð1×5Ò5°eÑ<Ô<Ò<Ð<Ð<Ð<Ø�<Š<˜ÑÔÐ 4× =Ò =¸cÑ BÔ BÒBÐBÐBÐBàŒ?˜nÒ,Ð,Ð,Ð,ØÔ ,Ð0DÐ!EÒEÐEÐEÐEàÔ"Ð"Ð"Ð"å�:˜|Ñ,Ô,Ð,Ð,Ð,Ý�:˜vÑ&Ô&Ð&Ð&Ð&Ý�:˜wÑ'Ô'Ð'Ð'Ð'Ý�:˜yÑ)Ô)Ð)Ð)Ð)Ð)Ð)s   ÁA.Á.A2Á5A2c                  óæ  — t          d¦  «        } t          d¦  «        }t          d¦  «        }t          ¦   «         5  t	          d| ||¦  «        }d d d ¦  «         n# 1 swxY w Y   |j        }|j        }|                      |d¦  «         |                     |¦  «        |                      |¦  «        k    sJ ‚| 	                    |¦  «        |  	                    |¦  «        k    sJ ‚|j
        |k    sJ ‚t          |d¦  «        rJ ‚t          |d¦  «        sJ ‚t          |d¦  «        sJ ‚t          |d	¦  «        sJ ‚|j        t          d¦  «        |j        fk    sJ ‚|j        t          d¦  «        k    sJ ‚|j        rJ ‚t          |d
d
d
¦  «        |_        |j        t          |d
d
d
¦  «        k    sJ ‚|j        sJ ‚t          ¦   «         5  t	          d|¬¦  «        }d d d ¦  «         n# 1 swxY w Y   |j        rJ ‚|j                             d|j        ¦  «        }|t          |j        dd
d
¦  «        z  }||f|_        |j        ||fk    sJ ‚|j        t          d¦  «        k    sJ ‚|j        sJ ‚d S )NÚparticle_masscenterÚparticle_massÚparticle_frameÚparticle_bodyr   Ú_inertiar   r   r   r+   )r   r   )r
   r   r   r   r   r   r   r   r   r-   r   r/   r   r   Úcentral_inertiar.   Ú	locatenewÚx)r4   r5   r6   r7   r   r   r   Úpoint_inertias           r%   Útest_particle_bodyr=   3   s?  € åÐ 5Ñ6Ô6ÐÝ˜?Ñ+Ô+€MÝ#Ð$4Ñ5Ô5€NÝ	Ñ	!Ô	!ð <ð <Ý˜_Ð.AØ*¨Nñ<ô <ˆð<ð <ð <ñ <ô <ð <ð <ð <ð <ð <ð <øøøð <ð <ð <ð <ð Ô
"€CØÔ€EØ×Ò °Ñ2Ô2Ð2Ø�7Š7�5‰>Œ>Ð0×4Ò4°UÑ;Ô;Ò;Ð;Ð;Ð;Ø�<Š<˜ÑÔÐ 3× <Ò <¸SÑ AÔ AÒAÐAÐAÐAàÔ Ò.Ð.Ð.Ð.Ý�} jÑ1Ô1Ð1Ð1Ð1Ý�= 'Ñ*Ô*Ð*Ð*Ð*Ý�= ,Ñ/Ô/Ð/Ð/Ð/Ý�= &Ñ)Ô)Ð)Ð)Ð)ØÔ ¥V¨A¡Y¤Y°Ô0HÐ$IÒIÐIÐIÐIØÔ(­F°1©I¬IÒ5Ð5Ð5Ð5ØÔ)Ð)Ð)Ð)å$+¨N¸A¸qÀ!Ñ$DÔ$D€MÔ!ØÔ(­G°NÀAÀqÈ!Ñ,LÔ,LÒLÐLÐLÐLØÔ%Ð%Ð%Ð%å	Ñ	!Ô	!ð Bð BÝ˜_°=ÐAÑAÔAˆðBð Bð Bñ Bô Bð Bð Bð Bð Bð Bð Bøøøð Bð Bð Bð BàÔ)Ð)Ð)Ð)ØÔ$×.Ò.¨w¸¼ÑHÔH€EØ!¥G¨MÔ,?ÀÀAÀqÑ$IÔ$IÑI€MØ*¨EÐ2€MÔØÔ  ]°EÐ$:Ò:Ð:Ð:Ð:ØÔ(­F°1©I¬IÒ5Ð5Ð5Ð5ØÔ%Ð%Ð%Ð%Ð%Ð%s#   ¼AÁAÁ"AÇ
G(Ç(G,Ç/G,c                  ó  — t          d¦  «        } t          d¦  «        }t          d¦  «        }t          ¦   «         5  t	          d| ||¦  «        }d d d ¦  «         n# 1 swxY w Y   t          d¦  «        }||j        j        z  }|                     ||j        ¦  «         t          |j
        ¦  «        dk    sJ ‚|j                             |j        dz   d¦  «        }|                     |j        d¦  «         |j
        d         d         }|j        }|                     |¦  «        |                     |¦  «        k    sJ ‚|                     |¦  «        |                     |¦  «        k    sJ ‚|j
        d         d         |k    sJ ‚d S )	Nr4   r5   r6   r7   Úar+   Ú_point0r   )r
   r   r   r   r   r   r;   Úapply_forcer   Úlenr   r:   Ú_namer   r   r-   )	r4   r5   r6   r7   r?   Úforce_vectorr   Úforce_pointr   s	            r%   Útest_particle_body_add_forcerF   Y   sÆ  € åÐ 5Ñ6Ô6ÐÝ˜?Ñ+Ô+€MÝ#Ð$4Ñ5Ô5€NÝ	Ñ	!Ô	!ð <ð <Ý˜_Ð.AØ*¨Nñ<ô <ˆð<ð <ð <ñ <ô <ð <ð <ð <ð <ð <ð <øøøð <ð <ð <ð <õ 	ˆs‰Œ€AØ�}Ô*Ô,Ñ,€LØ×Ò˜l¨MÔ,DÑEÔEÐEÝˆ}Ô"Ñ#Ô# qÒ(Ð(Ð(Ð(ØÔ$×.Ò.ØÔ˜iÑ'¨ñ,ô ,€Eà	‡M‚M�-Ô% qÑ)Ô)Ð)ØÔ% aÔ(¨Ô+€KàÔ€EØ�?Š?˜5Ñ!Ô! U§Y¢Y¨uÑ%5Ô%5Ò5Ð5Ð5Ð5Ø×Ò Ñ,Ô,°·²¸{Ñ0KÔ0KÒKÐKÐKÐKàÔ˜qÔ! !Ô$¨Ò4Ð4Ð4Ð4Ð4Ð4s   ¼AÁAÁ"Ac                  óJ  ‡	‡
— t          d¦  «        } t          d¦  «        }t          d¦  «        }t          |ddd¦  «        }t	          ¦   «         5  t          d| |||¦  «        Š
d d d ¦  «         n# 1 swxY w Y   t          d¦  «        }t          d¦  «        }‰
j                             d	|‰
j        j	        z  ¦  «        }| 
                    ‰
j        d¦  «         |‰
j        j        z  Š	‰
                     ‰	|¦  «         t          ‰
j        ¦  «        dk    sJ ‚‰
j        d         d         }‰
j        }|                     |¦  «        |                     |¦  «        k    sJ ‚|                     |¦  «        |                     |¦  «        k    sJ ‚‰
j        d         d         ‰	k    sJ ‚‰
                     ‰	¦  «         t          ‰
j        ¦  «        d
k    sJ ‚‰
j        d         d         ‰	k    sJ ‚t#          t$          ˆ	ˆ
fd„¦  «         t#          t$          ˆ
fd„¦  «         d S )Nr(   r)   r*   r+   r   r,   ÚlÚFaÚrigidbody_body_point0é   c                  ó0   •— ‰                      ‰ d¦  «        S ©Nr   ©rA   )rD   r1   s   €€r%   ú<lambda>z%test_body_add_force.<locals>.<lambda>�   s   ø€ ˜j×4Ò4°\ÀAÑFÔF€ ó    c                  ó.   •— ‰                       d¦  «        S rM   rN   )r1   s   €r%   rO   z%test_body_add_force.<locals>.<lambda>‘   s   ø€ ˜j×4Ò4°QÑ7Ô7€ rP   )r
   r   r   r   r   r   r   r:   r   r;   r   ÚzrA   rB   r   r   r-   r   Ú	TypeError)r(   r)   r*   r0   rH   rI   r   rE   r   rD   r1   s            @@r%   Útest_body_add_forcerT   r   s…  øø€ å Ð!7Ñ8Ô8ÐÝÐ,Ñ-Ô-€NÝ$Ð%6Ñ7Ô7€OÝ˜?¨A¨q°!Ñ4Ô4€LÝ	Ñ	!Ô	!ð Ið IÝÐ*Ð,@Ø(¨/¸<ñIô Iˆ
ðIð Ið Iñ Iô Ið Ið Ið Ið Ið Ið Iøøøð Ið Ið Ið Iõ 	ˆs‰Œ€AÝ	�‰Œ€BØÔ!×+Ò+ØØ	ˆJÔÔÑñ ô  €Eð 
‡M‚M�*Ô" AÑ&Ô&Ð&Ø˜
Ô(Ô*Ñ*€Là×Ò˜<¨Ñ/Ô/Ð/ÝˆzÔÑ Ô  AÒ%Ð%Ð%Ð%ØÔ" 1Ô% aÔ(€KØÔ€EØ�?Š?˜5Ñ!Ô! U§Y¢Y¨uÑ%5Ô%5Ò5Ð5Ð5Ð5Ø×Ò Ñ,Ô,°·²¸{Ñ0KÔ0KÒKÐKÐKÐKØÔ˜AÔ˜qÔ! \Ò1Ð1Ð1Ð1à×Ò˜<Ñ(Ô(Ð(ÝˆzÔÑ Ô  AÒ%Ð%Ð%Ð%ØÔ˜AÔ˜qÔ! \Ò1Ð1Ð1Ð1å
�9ÐFÐFÐFÐFÐFÑGÔGÐGÝ
�9Ð7Ð7Ð7Ð7Ñ8Ô8Ð8Ð8Ð8s   ÁA0Á0A4Á7A4c                  óL  ‡— t          ¦   «         5  t          d¦  «        Šd d d ¦  «         n# 1 swxY w Y   ‰j        j        } ‰                     | ¦  «         t          ‰j        ¦  «        dk    sJ ‚‰j        d         ‰j        | fk    sJ ‚t          t          ˆfd„¦  «         d S )Nr   r+   r   c                  ó.   •— ‰                       d¦  «        S rM   )Úapply_torque)r   s   €r%   rO   z&test_body_add_torque.<locals>.<lambda>›   s   ø€ ˜d×/Ò/°Ñ2Ô2€ rP   )	r   r   r   r;   rW   rB   r   r   rS   )Útorque_vectorr   s    @r%   Útest_body_add_torquerY   “   sã   ø€ Ý	Ñ	!Ô	!ð ð Ý�F‰|Œ|ˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð à”J”L€MØ×Ò�mÑ$Ô$Ð$åˆtŒz‰?Œ?˜aÒÐÐÐØŒ:�aŒ=˜TœZ¨Ð7Ò7Ð7Ð7Ð7Ý
�9Ð2Ð2Ð2Ð2Ñ3Ô3Ð3Ð3Ð3s   �,¬0³0c                  ó¼  — t          ¦   «         5  t          d¦  «        } d d d ¦  «         n# 1 swxY w Y   t          d¦  «        }t          ¦   «         5  t          d|¬¦  «        }d d d ¦  «         n# 1 swxY w Y   | j                             ||j        ¦  «         |                      |¦  «        |j        k    sJ ‚|                      |¦  «        |j        k    sJ ‚d S ©NÚAÚNÚB©r   )r   r   r   r   r   rR   Úmasscenter_vel©r\   r]   r^   s      r%   Útest_body_masscenter_velrb   �   sP  € Ý	Ñ	!Ô	!ð ð Ý�‰IŒIˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð å�sÑÔ€AÝ	Ñ	!Ô	!ð ð Ý�˜AÐÑÔˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð à„L×Ò˜˜AœCÑ Ô Ð Ø×Ò˜AÑÔ !¤#Ò%Ð%Ð%Ð%Ø×Ò˜AÑÔ !¤#Ò%Ð%Ð%Ð%Ð%Ð%ó   �+«/²/ÁA1Á1A5Á8A5c                  óþ  — t          ¦   «         5  t          d¦  «        } d d d ¦  «         n# 1 swxY w Y   t          d¦  «        }t          ¦   «         5  t          d|¬¦  «        }d d d ¦  «         n# 1 swxY w Y   | j                             ||j        ¦  «         |                      |¦  «        |j        k    sJ ‚|                     | ¦  «        |j         k    sJ ‚|                      |¦  «        |j        k    sJ ‚d S r[   )r   r   r   r   Úset_ang_velÚyÚ
ang_vel_inra   s      r%   Útest_body_ang_velrh   §   sg  € Ý	Ñ	!Ô	!ð ð Ý�‰IŒIˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð å�sÑÔ€AÝ	Ñ	!Ô	!ð ð Ý�˜AÐÑÔˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð à„G×Ò˜˜1œ3ÑÔÐØ�<Š<˜‰?Œ?˜aœcÒ!Ð!Ð!Ð!Ø�<Š<˜‰?Œ?˜qœs˜dÒ"Ð"Ð"Ð"Ø�<Š<˜‰?Œ?˜aœcÒ!Ð!Ð!Ð!Ð!Ð!rc   c                  óŽ  — t          ¦   «         5  t          d¦  «        } t          d¦  «        }d d d ¦  «         n# 1 swxY w Y   | j                             |j        |j        j        d¦  «         |                      |¦  «        t          t          d¦  «        t          d¦  «        dgt          d¦  «         t          d¦  «        dgg d¢g¦  «        k    sJ ‚|                      |j        ¦  «        t          t          d¦  «        t          d¦  «        dgt          d¦  «         t          d¦  «        dgg d¢g¦  «        k    sJ ‚d S )Nr\   r^   é
   r   )r   r   r+   )	r   r   r   Úorient_axisrR   Údcmr   r   r   )r\   r^   s     r%   Útest_body_dcmrm   ²   sS  € Ý	Ñ	!Ô	!ð ð Ý�‰IŒIˆÝ�‰IŒIˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð ð „G×Ò˜œ ¤¤¨BÑ/Ô/Ð/Ø�5Š5�‰8Œ8•v¥ B¡¤­¨R©¬°!Ð4½¸B¹¼°xÅÀRÁÄÈ!Ð6LÈiÈiÈiÐXÑYÔYÒYÐYÐYÐYØ�5Š5�”‰>Œ>�V¥c¨"¡g¤g­s°2©w¬w¸Ð%:½cÀ"¹g¼g¸XÅsÈ2ÁwÄwÐPQÐ<RÐT]ÐT]ÐT]Ð$^Ñ_Ô_Ò_Ð_Ð_Ð_Ð_Ð_s   �:º>Á>c                  óþ   — t          d¦  «        } t          ¦   «         5  t          d| ¬¦  «        }d d d ¦  «         n# 1 swxY w Y   |j        | j        k    sJ ‚|j        | j        k    sJ ‚|j        | j        k    sJ ‚d S )Nr]   r^   r_   )r   r   r   r;   rf   rR   )r]   r^   s     r%   Útest_body_axisro   º   s¹   € Ý�sÑÔ€AÝ	Ñ	!Ô	!ð ð Ý�˜AÐÑÔˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð àŒ3�!”#Š:ˆ:ˆ:ˆ:ØŒ3�!”#Š:ˆ:ˆ:ˆ:ØŒ3�!”#Š:ˆ:ˆ:ˆ:ˆ:ˆ:s   ž<¼A ÁA c                  óx  — t          d¦  «        \  } }t          d¦  «        }t          ¦   «         5  t          d¦  «        }d d d ¦  «         n# 1 swxY w Y   | |j        z  }||j        z  }|                     ||¦  «         |j        ||fgk    sJ ‚|                     ||¦  «         |j        |||z   fgk    sJ ‚d S )Nza bÚPr^   )r   r
   r   r   r;   rf   rA   r   )r?   Úbrq   r^   Úf1Úf2s         r%   Ú#test_apply_force_multiple_one_pointru   Â   s  € Ý�5‰>Œ>�D€A€qÝˆc‰
Œ
€AÝ	Ñ	!Ô	!ð ð Ý�‰IŒIˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð à	
ˆ1Œ3‰€BØ	
ˆ1Œ3‰€BØ‡M‚M�"�aÑÔÐØŒ7˜˜2�w�iÒÐÐÐØ‡M‚M�"�aÑÔÐØŒ7˜˜2˜b™5�z�lÒ"Ð"Ð"Ð"Ð"Ð"s   °AÁAÁAc                  óü  — t          d¦  «        \  } }t          d¦  «        \  }}}}t          d¦  «        }t          d¦  «        }t          ¦   «         5  t	          d¦  «        }t	          d¦  «        }	d d d ¦  «         n# 1 swxY w Y   t          d¦  «        }
|                     |j        ||j        z  ¦  «         |                     |	j        ||	j        z  ¦  «         | |z  |
j	        z  }| 
                    |||	|¦  «         |j        ||fgk    sJ ‚|	j        || fgk    sJ ‚|j        |z  |
j        z  }|	j        |z  |
j        z  }| 
                    |¦  «         |	 
                    |¦  «         |j        ||f|j        |fgk    sJ ‚|	j        || f|	j        |fgk    sJ ‚||
j        z  }| 
                    ||	¬¦  «         |j        ||f|j        ||z   fgk    sJ ‚|	j        || f|	j        | |z   fgk    sJ ‚d S )	Nzf gz	q x v1 v2ÚP1ÚP2ÚB1ÚB2r]   ©Úreaction_body)r   r   r
   r   r   r   r   r   r;   rR   rA   r   r   rf   r   )ÚfÚgÚqr;   Úv1Úv2rw   rx   ry   rz   r]   ÚforceÚg1Úg2Úforce2s                  r%   Útest_apply_forcer†   Î   s\  € Ý�5‰>Œ>�D€A€qÝ! +Ñ.Ô.�L€A€qˆ"ˆbÝ	ˆt‰Œ€BÝ	ˆt‰Œ€BÝ	Ñ	!Ô	!ð ð Ý�$‰ZŒZˆÝ�$‰ZŒZˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð õ 	�sÑÔ€Aà‡J‚JˆrŒx˜˜BœD™Ñ!Ô!Ð!Ø‡J‚JˆrŒx˜˜BœD™Ñ!Ô!Ð!Øˆa‰C�”‰G€Eà‡N‚N�5˜"˜b "Ñ%Ô%Ð%ØŒ8˜˜U˜�}Ò$Ð$Ð$Ð$ØŒ8˜˜e˜V˜�~Ò%Ð%Ð%Ð%à	Œ�‰�1”3‰€BØ	Œ�‰�1”3‰€Bà‡N‚N�2ÑÔÐØ‡N‚N�2ÑÔÐàŒ8˜˜E˜
 R¤]°BÐ$7Ð8Ò8Ð8Ð8Ð8ØŒ8˜˜e˜V˜ r¤}°bÐ&9Ð:Ò:Ð:Ð:Ð:àˆqŒs‰U€Fà‡N‚N�6¨€NÑ,Ô,Ð,àŒ8˜˜U˜ b¤m°V¸B±YÐ%?Ð@Ò@Ð@Ð@Ð@ØŒ8˜˜e˜V˜ r¤}°v°g¸b±jÐ&AÐBÒBÐBÐBÐBÐBÐBs   ÁA>Á>BÂBc                  óò  — t          d¦  «        } t          d¦  «        }t          ¦   «         5  t          d¦  «        }t          d¦  «        }d d d ¦  «         n# 1 swxY w Y   t	          d¦  «        }| |z  |j        z  }|                     ||¦  «         |j        |j        |fgk    sJ ‚|j        |j        | fgk    sJ ‚| |j	        z  }|                     |¦  «         |j        |j        ||z   fgk    sJ ‚d S )NÚtr   ry   rz   r]   )
r   r   r   r   r   r;   rW   r   r   rf   )rˆ   r   ry   rz   r]   ÚtorqueÚtorque2s          r%   Útest_apply_torquer‹   ð   sC  € Ý�‰Œ€AÝ�sÑÔ€AÝ	Ñ	!Ô	!ð ð Ý�$‰ZŒZˆÝ�$‰ZŒZˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð õ 	�sÑÔ€AØˆq‰S�”‰W€Fà‡O‚O�F˜BÑÔÐØŒ8˜œ 6Ð*Ð+Ò+Ð+Ð+Ð+ØŒ8˜œ F 7Ð+Ð,Ò,Ð,Ð,Ð,à�”‰e€GØ‡O‚O�GÑÔÐØŒ8˜œ 6¨'¡>Ð2Ð3Ò3Ð3Ð3Ð3Ð3Ð3s   ­AÁAÁAc                  óN  — t          d¦  «        } t          d¦  «        }t          ¦   «         5  t          d¦  «        }d d d ¦  «         n# 1 swxY w Y   | |j        z  }|                     ||¦  «         |j        ||fgk    sJ ‚|                     ¦   «          |j        g k    sJ ‚d S )Nr?   rq   r^   )r   r
   r   r   rR   rA   r   Úclear_loads)r?   rq   r^   r‚   s       r%   Útest_clear_loadrŽ     sÜ   € Ý�‰Œ€AÝˆc‰
Œ
€AÝ	Ñ	!Ô	!ð ð Ý�‰IŒIˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð àˆaŒc‰E€EØ‡M‚M�%˜ÑÔÐØŒ7˜˜5�z�lÒ"Ð"Ð"Ð"Ø‡M‚M�O„O€OØŒ7�bŠ=ˆ=ˆ=ˆ=ˆ=ˆ=ó   ­A	Á	AÁAc                  ó~  — t          d¦  «        } t          d¦  «        }t          ¦   «         5  t          d¦  «        }d d d ¦  «         n# 1 swxY w Y   |j        }|j        }|                     || ¦  «         |                     ||¦  «         |j        | |f||fgk    sJ ‚|                     |¦  «         |j        | |fgk    sJ ‚|                     | 	                    |¦  «        ¦  «         |j        | |f|j
        | 	                    |¦  «        fgk    sJ ‚|                     ¦   «          |j        | |fgk    sJ ‚d S )Nrw   rx   r^   )r
   r   r   r;   rf   rA   r   Úremove_loadrW   Úcrossr   )rw   rx   r^   rs   rt   s        r%   Útest_remove_loadr“     sz  € Ý	ˆt‰Œ€BÝ	ˆt‰Œ€BÝ	Ñ	!Ô	!ð ð Ý�‰IŒIˆðð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð à	
Œ€BØ	
Œ€BØ‡M‚M�"�bÑÔÐØ‡M‚M�"�bÑÔÐØŒ7˜˜B�x " b Ð*Ò*Ð*Ð*Ð*Ø‡M‚M�"ÑÔÐØŒ7˜˜B�x�jÒ Ð Ð Ð Ø‡N‚N�2—8’8˜B‘<”<Ñ Ô Ð ØŒ7˜˜B�x !¤'¨2¯8ª8°B©<¬<Ð!8Ð9Ò9Ð9Ð9Ð9Ø‡M‚M�O„O€OØŒ7˜˜B�x�jÒ Ð Ð Ð Ð Ð r�   c                  ó   — t          ¦   «         5  t          d¦  «        } t          d¦  «        }t          d¦  «        }t          d¦  «        }ddd¦  «         n# 1 swxY w Y   t          d¦  «        \  }}t          d¦  «        \  }}}}	t          d¦  «        \  }
}|                     |
| j        z  ¦  «         |                      ||z  | j        z  |¬	¦  «         |                      ||                     ¦   «         z  | j        z  |¬	¦  «         |                     |j        |z  | j        z  ¦  «         |                     |j        |z  | j        z  ¦  «         | 	                    |	|z  | j
        z  |¬	¦  «         | 	                    || j
        z  ¦  «         |j        |j        |
||z  z
  ||                     ¦   «         z  z
  | j        z  fgk    sJ ‚|j        |j        |j        |z  | j        z  f|j        ||	|z  z   | j
        z  fgk    sJ ‚|j        |j        |j        |z  | j        z  f|j        |	 |z  | j
        z  fgk    sJ ‚| j        | j        ||                     ¦   «         z  ||z  z   | j        z  fgk    sJ ‚dS )
zXExample based on: https://pydy.readthedocs.io/en/latest/examples/multidof-holonomic.htmlÚWr^   rq   rr   Nzq1 q2zk c g kTzF Tr{   )r   r   r   r   rA   r;   Údiffr   rf   rW   rR   r   r   r   )r•   r^   rq   rr   Úq1Úq2ÚkÚcr~   ÚkTÚFÚTs               r%   Ú9test_apply_loads_on_multi_degree_freedom_holonomic_systemrž     sš  € å	Ñ	!Ô	!ð ð Ý�‰IŒIˆÝ�‰IŒIˆÝ�‰IŒIˆÝ�‰IŒIˆð	ð ð ñ ô ð ð ð ð ð ð øøøð ð ð ð õ
 ˜GÑ$Ô$�F€BˆÝ˜*Ñ%Ô%�K€A€qˆ!ˆRÝ˜%Ñ Ô �D€A€qð ‡M‚M�!�A”C‘%ÑÔÐØ‡M‚M�!�B‘$�q”s‘(¨!€MÑ,Ô,Ð,Ø‡M‚M�!�B—G’G‘I”I‘+˜aœc‘/°€MÑ3Ô3Ð3Ø‡M‚M�!”&˜‘(˜1œ3‘,ÑÔÐØ‡M‚M�!”&˜‘(˜1œ3‘,ÑÔÐð ‡N‚N�2�b‘5˜œ‘9¨A€NÑ.Ô.Ð.Ø‡N‚N�1�Q”S‘5ÑÔÐàŒ7˜œ q¨1¨R©4¡x°!°B·G²G±I´I±+Ñ'=¸q¼sÑ&BÐCÐDÒDÐDÐDÐDØŒ7˜œ a¤f¨Q¡h¨q¬s¡lÐ3°a´gÀÀBÀrÁEÁ	È1Ì3¹Ð5OÐPÒPÐPÐPÐPØŒ7˜œ a¤f¨Q¡h¨q¬s¡lÐ3°a´gÀ¸sÀ2¹vÀaÄc¹zÐ5JÐKÒKÐKÐKÐKØŒ7˜œ q¨¯ª©¬¡{°Q°r±TÑ'9¸1¼3Ñ&>Ð?Ð@Ò@Ð@Ð@Ð@Ð@Ð@s   �=AÁAÁAc            
      ó�  — t          d¦  «        } t          d¦  «        \  }}}}}}t          | |||¦  «        }t          d¦  «        }|                     d|| j        z  || j        z  z   ¦  «        }	t          ¦   «         5  t          d|| ||¬¦  «        }
d d d ¦  «         n# 1 swxY w Y   |
 	                    |	¦  «        }t          | |||dz  z  z   |||dz  z  z   |||dz  |dz  z   z  z   | |z  |z  ¬¦  «        }||k    sJ ‚t          d	¦  «        }| 
                    | | j        d
¦  «         t          |
 	                    |	|¦  «        |z
                       |¦  «        ¦  «        t          dd¦  «        k    sJ ‚t          d¦  «        }|                     d|| j        z  || j        z  z   ¦  «        }	t          ¦   «         5  t          d||| ¬¦  «        }d d d ¦  «         n# 1 swxY w Y   | 	                    |	| ¦  «        }t          | ||dz  z  ||dz  z  ||dz  |dz  z   z  | |z  |z  ¬¦  «        }|j        rJ ‚||k    sJ ‚d S )Nr]   zm, I_x, I_y, I_z, a, bÚoÚpÚR)r   r   r   r9   rK   )r"   r\   r+   é   rq   )r   r   r   )r   r   r   r
   r:   r;   rf   r   r   Úparallel_axisrk   rR   r	   Ú	to_matrixr   r.   )r]   ÚmÚIxÚIyÚIzr?   rr   ÚIor    r¡   r¢   ÚIpÚIp_expectedr\   rq   s                  r%   Útest_parallel_axisr­   9  s  € Ý�sÑÔ€AÝ!Ð":Ñ;Ô;Ñ€A€rˆ2ˆr�1�aÝ	��B˜˜BÑ	Ô	€Båˆc‰
Œ
€AØ	�Š�C˜˜QœS™ 1 q¤s¡7Ñ*Ñ+Ô+€AÝ	Ñ	!Ô	!ð Ið IÝ� ¨!°!ÀRÐHÑHÔHˆðIð Ið Iñ Iô Ið Ið Ið Ið Ið Ið Iøøøð Ið Ið Ið Ià	
�Š˜Ñ	Ô	€BÝ˜!˜R ! a¨¡d¡(™]¨B°°Q¸±T±©MØ˜q A q¡D¨1¨a©4¡KÑ0Ñ0°q°b¸1±f¸q±jðBñ Bô B€Kà�ÒÐÐÐå�sÑÔ€AØ‡M‚M�!�Q”S˜!ÑÔÐÝØ	
�Š˜˜AÑ	Ô	 Ñ	,×7Ò7¸Ñ:Ô:ñ<ô <Ý?DÀQÈ¹{¼{òKð Kð Kð Kõ 	ˆc‰
Œ
€AØ	�Š�C˜˜QœS™ 1 q¤s¡7Ñ*Ñ+Ô+€AÝ	Ñ	!Ô	!ð 5ð 5Ý� ¨°!Ð4Ñ4Ô4ˆð5ð 5ð 5ñ 5ô 5ð 5ð 5ð 5ð 5ð 5ð 5øøøð 5ð 5ð 5ð 5à	
�Š˜˜AÑ	Ô	€BÝ˜!˜Q  a¡™Z¨¨Q°!©V©°Q¸!¸q¹&À1ÈÁ6¹/Ñ5JØ ˜b 1™f q™jð*ñ *ô *€KàŒ~ÐÐÐØ�ÒÐÐÐÐÐs$   Á>BÂB#Â&B#ÇG!Ç!G%Ç(G%N)$Úsympyr   r   r   r   r   r   r	   Úsympy.physics.vectorr
   r   r   r   Úsympy.physics.mechanicsr   r   Úsympy.testing.pytestr   r   r&   r2   r=   rF   rT   rY   rb   rh   rm   ro   ru   r†   r‹   rŽ   r“   rž   r­   © rP   r%   ú<module>r³      sÈ  ðð*ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *ð *à NÐ NÐ NÐ NÐ NÐ NÐ NÐ NÐ NÐ NÐ NÐ NØ 1Ð 1Ð 1Ð 1Ð 1Ð 1Ð 1Ð 1Ø ?Ð ?Ð ?Ð ?Ð ?Ð ?Ð ?Ð ?ð-ð -ð -ð"*ð *ð *ð4#&ð #&ð #&ðL5ð 5ð 5ð29ð 9ð 9ðB4ð 4ð 4ð&ð &ð &ð	"ð 	"ð 	"ð`ð `ð `ðð ð ð
#ð 
#ð 
#ð Cð  Cð  CðD4ð 4ð 4ð"	ð 	ð 	ð!ð !ð !ð"Að Að Að8ð ð ð ð rP   