Ë
    7^(hK•  ã                   óR  — d dl 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
 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mZmZ d dlmZ d d	lmZ ej>                  Z  ed
«      Z! ed
d«      Z" ed«      Z# ed«      Z$ G d„ d«      Z% G d„ de%«      Z& G d„ de%«      Z' G d„ d«      Z(y)é    N)Úsymbols)Úsympify)ÚcosÚsin)ÚeyeÚzeros)ÚImmutableMatrix)ÚForceÚKanesMethodÚLagrangesMethodÚParticleÚPinJointÚPointÚPrismaticJointÚReferenceFrameÚ	RigidBodyÚTorqueÚTorqueActuatorÚSystemÚdynamicsymbols)Úsimplify)Úsolvezq:6é   úu:6zua:3c                   óÜ   — e Zd Z ej                  «       d„ «       Zd	d„Zd
d„Z ej                  «       d„ «       Z ej                  «       d„ «       Z	d	d„Z
 ej                  «       d„ «       Zy)ÚTestSystemBasec                 óJ   — t        t        d«      t        d«      «      | _        y )NÚframeÚfixed_point)r   r   r   Úsystem©Úselfs    úm/var/www/skyplay_api_hub/venv/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_system_class.pyÚ_empty_system_setupz"TestSystemBase._empty_system_setup   s   € äœ^¨GÓ4´e¸MÓ6JÓKˆ�ó    c                 óè   — d}d}|D ]%  }||vsŒt        | j                  |«      d d  g k(  rŒ%J ‚ |D ]"  }||vsŒt        | j                  |«      dk(  rŒ"J ‚ d|vr| j                  j                  �J ‚y y )N)
Úq_indÚq_depÚqÚu_indÚu_depÚuÚu_auxÚkdesÚholonomic_constraintsÚnonholonomic_constraints)ÚloadsÚbodiesÚjointsÚ	actuators© Ú
eom_method)Úgetattrr    r6   )r"   ÚexcludeÚmatricesÚtuplesÚattrs        r#   Ú_empty_system_checkz"TestSystemBase._empty_system_check   s›   € ðQˆà;ˆØò 	;ˆDØ˜7Ò"Ü˜tŸ{™{¨DÓ1±!Ð4¸Ó:Ð:Ð:ð	;ð ò 	8ˆDØ˜7Ò"Ü˜tŸ{™{¨DÓ1°RÓ7Ð7Ð7ð	8ð ˜wÑ&Ø—;‘;×)Ñ)Ð1Ð1Ñ1ð 'r%   c           
      óJ  — t        t        d«      t        d«      «      | _        |rt	        d«      nt
        }t        dt        ¬«      | _        t        d| j                  d   | j                  d   t        d   |d   «      t        d	| j                  d   | j                  d
   t        d   |d   «      t        d| j                  d
   | j                  d   t        d
   |d
   «      f| _         | j                  j                  | j                  Ž  | j                  j                  t        d   dg¬«       | j                  j                  |d   d¬«       |rR| j                  j!                  |d   t
        d   z
  «       | j                  j#                  t$        d   t$        d   «       | j                  j'                  t        d
   t        d   z
  t        d   z   «       | j                  j)                  |d   t
        d   z
  |d
   z   «       |d d
 | j                  _        |d
d | j                  _        | j                  j.                  d d  | j                  j0                  d d  c| _        | _        | j                  j*                  d d  | j                  j,                  d d  c| _        | _        | j                  j2                  d d  | _        | j                  j4                  d d  | _        | j                  j8                  d d  | _        | j                  j<                  d d  | _        y )Nr   r   r   zrb1:5©ÚclsÚJ1r   r   ÚJ2é   ÚJ3é   F)Úindependenté   ) r   r   r   r    r   Úqdr   r   r2   r   r)   r   r3   Ú
add_jointsÚadd_coordinatesÚ
add_speedsÚadd_kdesÚadd_auxiliary_speedsÚuaÚadd_holonomic_constraintsÚadd_nonholonomic_constraintsr*   r+   r'   r(   r.   r/   ÚhcÚvelocity_constraintsÚvcr0   Únhc)r"   Úwith_speedsr,   s      r#   Ú_create_filled_systemz$TestSystemBase._create_filled_system(   sl  € Üœ^¨GÓ4´e¸MÓ6JÓKˆŒÙ%0ŒN˜5Ô!´bˆÜ˜g¬9Ô5ˆŒä�T˜4Ÿ;™; q™>¨4¯;©;°q©>¼1¸Q¹4ÀÀ1ÁÓFÜ˜4 §¡¨Q¡°·±¸Q±ÄÀ1ÁÀqÈÁtÓLÜ�T˜4Ÿ;™; q™>¨4¯;©;°q©>¼1¸Q¹4ÀÀ1ÁÓFð
ˆŒð
 	ˆ�‰×Ñ §¡Ñ,Ø�‰×#Ñ#¤A a¡D°u°gÐ#Ô>Ø�‰×Ñ˜q ™t°ÐÔ7ÙØ�K‰K× Ñ   1¡¬¨1©¡Ô.Ø�K‰K×,Ñ,¬R°©U´B°q±EÔ:Ø�‰×-Ñ-¬a°©d´Q°q±T©k¼A¸a¹DÑ.@ÔAØ�‰×0Ñ0°°1±¼¸1¹±ÀÀ!ÁÑ1DÔEØ˜b˜q˜Eˆ�‰ÔØ˜a ˜Fˆ�‰ÔØ!%§¡×!2Ñ!2±1Ð!5°t·{±{×7HÑ7HÉÐ7KÐˆŒ
�D”JØ!%§¡×!2Ñ!2±1Ð!5°t·{±{×7HÑ7HÉÐ7KÐˆŒ
�D”JØ—K‘K×$Ñ$¡QÐ'ˆŒ	Ø—+‘+×3Ñ3±AÐ6ˆŒØ—+‘+×2Ñ2±1Ð5ˆŒØ—;‘;×7Ñ7¹Ð:ˆ�r%   c                 ó(   — | j                  d¬«       y )NT©rT   ©rU   r!   s    r#   Ú_filled_system_setupz#TestSystemBase._filled_system_setupB   s   € à×"Ñ"¨tÐ"Õ4r%   c                 ó(   — | j                  d¬«       y )NFrW   rX   r!   s    r#   Ú_filled_system_setup_no_speedsz-TestSystemBase._filled_system_setup_no_speedsF   s   € à×"Ñ"¨uÐ"Õ5r%   c                 óf  — d|v s%| j                   j                  d d  t        d d k(  sJ ‚d|v s&| j                   j                  d d  t        d   gk(  sJ ‚d|v s%| j                   j                  d d  t        d d k(  sJ ‚d|v s%| j                   j                  d d  t
        d d k(  sJ ‚d|v s%| j                   j                  d d  t
        dd k(  sJ ‚d	|v s%| j                   j
                  d d  t
        d d k(  sJ ‚d
|v s%| j                   j                  d d  t        d d k(  sJ ‚d|v sL| j                   j                  d d  t        t
        d d t        d d «      D ��cg c]
  \  }}||z
  ‘Œ c}}k(  sJ ‚d|v s:| j                   j                  d d  t        d   t        d   z
  t        d   z   gk(  sJ ‚d|v s:| j                   j                  d d  t
        d   t        d   z
  t
        d   z   gk(  sJ ‚d|v sV| j                   j                  d d  t        d   t        d   z
  t        d   z   t
        d   t        d   z
  t
        d   z   gk(  sJ ‚d|v s.| j                   j                  t!        | j                  «      k(  sJ ‚d|v s/| j                   j"                  t!        | j"                  «      k(  sJ ‚y y c c}}w )Nr'   rD   r(   r)   rF   r*   rB   r+   r,   r-   r.   r/   r   r   r0   rQ   r2   r3   )r    r'   r)   r(   r*   r,   r+   r-   rM   r.   ÚziprG   r/   r0   rQ   r2   Útupler3   )r"   r8   ÚuiÚqdis       r#   Ú_filled_system_checkz#TestSystemBase._filled_system_checkJ   sÉ  € Ø˜'Ñ! T§[¡[×%6Ñ%6±qÐ%9¼Q¸rÀ¸UÒ%BÐBÐBØ˜'Ñ! T§[¡[×%6Ñ%6±qÐ%9¼aÀ¹d¸VÒ%CÐCÐCØ�g‰~ §¡§¡©qÐ!1´Q°r¸°UÒ!:Ð:Ð:Ø˜'Ñ! T§[¡[×%6Ñ%6±qÐ%9¼Q¸rÀ¸UÒ%BÐBÐBØ˜'Ñ! T§[¡[×%6Ñ%6±qÐ%9¼Q¸qÀ¸VÒ%CÐCÐCØ�g‰~ §¡§¡©qÐ!1´Q°r¸°UÒ!:Ð:Ð:Ø˜'Ñ! T§[¡[×%6Ñ%6±qÐ%9¼RÀÀ¸VÒ%CÐCÐCØ˜Ñ  D§K¡K×$4Ñ$4±QÐ$7Ü$'¬¨"¨1¨¬r°"°1¨vÓ$6÷<8Ù ˜˜SˆB�‹Hó<8ò %8ð 	8ð 8à'¨7Ñ2Ø—‘×1Ñ1±!Ð4¼¸1¹ÄÀ!Á¹ÄqÈÁtÑ9KÐ8LÒLð	Nð Mà*¨gÑ5Ø—‘×4Ñ4±QÐ7¼A¸a¹DÄ2ÀaÁ5¹LÌ1ÈQÉ4Ñ<OÐ;PÒPð	ð Qð '¨'Ñ1Ø—‘×0Ñ0±Ð3Ü�q‘EœB˜q™E‘M¤B q¡EÑ)¬1¨Q©4´"°Q±%©<¼!¸A¹$Ñ+>ð8@ò @ð	Að @ð ˜GÑ#Ø—‘×"Ñ"¤e¨D¯K©KÓ&8Ò8ð	:ð 9à˜GÑ#Ø—‘×"Ñ"¤e¨D¯K©KÓ&8Ò8ð	:ð 9Ø8ð $ùó<8s   ÅJ-c                 óÌ  — t         d   | j                  _        t        d   | j                  _        t        d   t         d   j                  t        «      z
  | j                  _        t        dt        d«      ¬«      }| j                  j                  |«       |j                  j                  | j                  j                  t         d   | j                  j                  z  «       y )Nr   ÚpÚm©Úmass)r)   r    r'   r,   r*   ÚdiffÚtr.   r   r   Ú
add_bodiesÚ
masscenterÚset_posr   Úx)r"   r$   rc   s      r#   Ú_moving_point_massz!TestSystemBase._moving_point_massa   s”   € ä˜a™Dˆ�‰ÔÜ˜a™Dˆ�‰ÔÜ˜Q™4¤! A¡$§)¡)¬A£,Ñ.ˆ�‰ÔÜ�Sœw s›|Ô,ˆØ�‰×Ñ˜qÔ!Ø	�‰×Ñ˜TŸ[™[×4Ñ4´a¸±d¸T¿[¹[¿]¹]Ñ6JÕKr%   N)r5   )T)Ú__name__Ú
__module__Ú__qualname__ÚpytestÚfixturer$   r<   rU   rY   r[   ra   rm   r5   r%   r#   r   r      s‡   „ Ø€V‡^�^ÓñLó ðLó2ó;ð4 €V‡^�^Óñ5ó ð5ð €V‡^�^Óñ6ó ð6ó:ð. €V‡^�^ÓñLó ñLr%   r   c                   óÚ  — e Zd Zd„ Zd„ Zej                  j                  dd ed«      g«      ej                  j                  dd e	d«      g«      d„ «       «       Z
d„ Zd„ Zej                  j                  d	edd
 i edd
 g edd
 fedd
 ddiedd
 g edd
 fedd
 ddig edd
 edd
 fedd
 dg d¢ied   ed   ged   ged   ed   ed   gfg«      d„ «       Zej                  j                  dddg«      ej                  j                  ded   ed   fi fed   ed   fi fed   fddifed   fddifed   ed   fi f ed«      ed   fi fg«      d„ «       «       Zej                  j                  dedd
 i edd
 g edd
 fedd
 ddiedd
 g edd
 fedd
 ddig edd
 edd
 fedd
 dg d¢ied   ed   ged   ged   ed   ed   gfg«      d„ «       Zej                  j                  dedd
 i edd
 fg«      d„ «       Zej                  j                  ded   ed   fi fed   ed   fi fed   ed   fi f ed«      ed   fi fg«      d„ «       Zej                  j                  dd ded   fi fd!ded
   fddifd"ded   fi fd#ded
   fddifd$d%ed   fi fd&d'ed   ed   z
  fi fd(d)ed   ed   z
  fi fd*d+ed   ed   z
  fi fd,d- ed.«      fi fd/d0 e e	d1«       ed2«      j2                  «      fi fd3d4 e ed5«       ed2«      j2                   ed6«      «      fi fg«      d7„ «       Zej                  j                  d8d d ed«      efd!d ed«      efd"d ed«      efd#d ed«      efd$d% ed«      efd&d'd9efd(d)d9efd*d+d9efd,d- ed«      efd/d0 ed«      efd3d4 ed«      efg«      d:„ «       Zej                  j                  d;d<i  eedd= edd= «      D � ��cg c]
  \  }}||z
  ‘Œ c}}} fed=   ed=   z
  ed   ed   z
  fi  eedd> edd> «      D � ��cg c]
  \  }}||z
  ‘Œ c}}} fg«      d?„ «       Z ej                  j                  ded   ed   z
  ed=   ed=   z
  fi fed   ed   z
   ed=   ed=   z
  fi fed   ed   z
  ed=   ed=   z
  gi fg«      d@„ «       Z!ej                  j                  dAd<i ed   ed   z
  ed   z   gfed=   ed   z
  ed   ed
   z   fi ed   ed   z
  ed   z   ed=   ed   z
  ed   ed
   z   gfg«      dB„ «       Z"ej                  j                  ded   ed   z
  ed   z   ed=   ed
   z
  fi fed   ed   z
  ed   z    ed=   ed
   z
  fi fed   ed   z
  ed=   ed
   z
  fi fg«      dC„ «       Z#ej                  j                  dAd<i ed
   ed   z
  ed   z   gfed=   ed   z
  ed   ed
   z   fi ed
   ed   z
  ed   z   ed=   ed   z
  ed   ed
   z   gfg«      dD„ «       Z$ej                  j                  ded
   ed   z
  ed   z   ed=   ed
   z
  fi fed
   ed   z
  ed   z    ed=   ed
   z
  fi fed   ed   z
  ed=   ed
   z
  fi fed   ed   z
  ed=   ed
   z
  gi fg«      dE„ «       Z%ej                  j                  dFg g fed   ed   z
  ed   z   ed   ed   z
  ed   z   gfed   ed   z   ed   ed   z
  ged   ed   z   ed   ed   z
  gfg«      dG„ «       Z&dH„ Z'dI„ Z(dJ„ Z)dK„ Z*dL„ Z+dM„ Z,ej                  j                  dNg dO¢«      dP„ «       Z-ej                  j                  dQg dR¢«      dS„ «       Z.ej                  j                  dTe/e0g«      dU„ «       Z1ej                  j                  dVi  e2dWdgd edX«      gg«      fdYdi e2ddgd edX«      gg«      fg«      dZ„ «       Z3ej                  j                  d[i  e2ddgd edX«      gg«       e2ed   ji                  e5«      dg«      fg«      d\„ «       Z6ej                  j                  d]e/d^diefe0d^diefe/d,g iefe/d_g iefe0d,g iefe0d`diefg«      da„ «       Z7yc c}}} w c c}}} w )bÚ
TestSystemc                 óX   — | j                  «        | j                  j                  «        y ©N)r<   r    Úvalidate_system)r"   r$   s     r#   Útest_empty_systemzTestSystem.test_empty_systeml   s   € Ø× Ñ Ô"Ø�‰×#Ñ#Õ%r%   c                 óX   — | j                  «        | j                  j                  «        y rv   )ra   r    rw   ©r"   rY   s     r#   Útest_filled_systemzTestSystem.test_filled_systemp   s   € Ø×!Ñ!Ô#Ø�‰×#Ñ#Õ%r%   r   Nr   c                 ó"  — |€|€t        «       | _        nt        ||«      | _        |€%| j                  j                  j                  dk(  sJ ‚| j                  j                  |k(  sJ ‚|€%| j                  j                  j                  dk(  sJ ‚| j                  j                  |k(  sJ ‚| j                  «        t        | j                  j                  t        «      sJ ‚t        | j                  j                  t        «      sJ ‚t        | j                  j                  t        «      sJ ‚t        | j                  j                  t        «      sJ ‚t        | j                  j                  t        «      sJ ‚t        | j                  j                  t        «      sJ ‚t        | j                  j                  t        «      sJ ‚t        | j                  j                  t        «      sJ ‚t        | j                  j                   t        «      sJ ‚y )NÚinertial_pointÚinertial_frame)r   r    r   Únamer   r<   Ú
isinstancer'   r	   r(   r)   r*   r+   r,   r.   r/   r0   )r"   r   r   s      r#   Ú	test_initzTestSystem.test_initt   s�  € ð Ð 5 =Ü ›(ˆD�Kä  ¨Ó4ˆDŒKØÐØ—;‘;×*Ñ*×/Ñ/Ð3CÒCÐCÐCà—;‘;×*Ñ*¨kÒ9Ð9Ð9Øˆ=Ø—;‘;×$Ñ$×)Ñ)Ð-=Ò=Ð=Ð=à—;‘;×$Ñ$¨Ò-Ð-Ð-Ø× Ñ Ô"Ü˜$Ÿ+™+×+Ñ+¬_Ô=Ð=Ð=Ü˜$Ÿ+™+×+Ñ+¬_Ô=Ð=Ð=Ü˜$Ÿ+™+Ÿ-™-¬Ô9Ð9Ð9Ü˜$Ÿ+™+×+Ñ+¬_Ô=Ð=Ð=Ü˜$Ÿ+™+×+Ñ+¬_Ô=Ð=Ð=Ü˜$Ÿ+™+Ÿ-™-¬Ô9Ð9Ð9Ü˜$Ÿ+™+×*Ñ*¬OÔ<Ð<Ð<Ü˜$Ÿ+™+×;Ñ;¼_ÔMÐMÐMÜ˜$Ÿ+™+×>Ñ>ÄÔPÐPÑPr%   c                 ó*  — t        d«      }t        j                  |«      | _        | j                  j                  |j
                  k(  sJ ‚| j                  j                  |j                  k(  sJ ‚| j                  d¬«       |f| j                  _        y )NÚbody)r2   ©r8   )	r   r   Úfrom_newtonianr    r   rj   r   r<   r2   )r"   Úrbs     r#   Útest_from_newtonian_rigid_bodyz)TestSystem.test_from_newtonian_rigid_bodyŽ   su   € Ü�vÓˆÜ×+Ñ+¨BÓ/ˆŒØ�{‰{×&Ñ&¨"¯-©-Ò7Ð7Ð7Ø�{‰{× Ñ  B§H¡HÒ,Ð,Ð,Ø× Ñ ¨Ð Ô5Ø ˜Uˆ�‰Õr%   c                 ó    — t        d«      }t        j                  t        «      5  t	        j
                  |«       d d d «       y # 1 sw Y   y xY w)NÚparticle)r   rq   ÚraisesÚ	TypeErrorr   r…   )r"   Úpts     r#   Útest_from_newtonian_particlez'TestSystem.test_from_newtonian_particle–   s;   € Ü�jÓ!ˆÜ�]‰]œ9Ó%ñ 	&Ü×!Ñ! "Ô%÷	&÷ 	&ñ 	&ús   ¥AÁAz)args, kwargs, exp_q_ind, exp_q_dep, exp_qrD   rE   TF)TFTr   rB   r   c                 ó0  —  | j                   j                  |i |¤Ž | j                   j                  d d  |k(  sJ ‚| j                   j                  d d  |k(  sJ ‚| j                   j                  d d  |k(  sJ ‚| j                  d¬«       || j                   _        || j                   _        | j                   j                  d d  |k(  sJ ‚| j                   j                  d d  |k(  sJ ‚| j                   j                  d d  |k(  sJ ‚| j                  d¬«       y )N)r'   r(   r)   r„   )r    rI   r'   r(   r)   r<   )r"   r$   ÚargsÚkwargsÚ	exp_q_indÚ	exp_q_depÚexp_qs          r#   Útest_coordinateszTestSystem.test_coordinates›   s  € ð 	$ˆ�‰×#Ñ# TÐ4¨VÒ4Ø�{‰{× Ñ ¡Ð# yÒ0Ð0Ð0Ø�{‰{× Ñ ¡Ð# yÒ0Ð0Ð0Ø�{‰{�}‰}™QÐ 5Ò(Ð(Ð(Ø× Ñ Ð)@Ð ÔAà%ˆ�‰ÔØ%ˆ�‰ÔØ�{‰{× Ñ ¡Ð# yÒ0Ð0Ð0Ø�{‰{× Ñ ¡Ð# yÒ0Ð0Ð0Ø�{‰{�}‰}™QÐ 5Ò(Ð(Ð(Ø× Ñ Ð)@Ð ÕAr%   ÚfuncrI   rJ   zargs, kwargsé   Úac                 ó¸   — t        j                  t        «      5   t        | j                  |«      |i |¤Ž d d d «       | j                  «        y # 1 sw Y   ŒxY wrv   )rq   rŠ   Ú
ValueErrorr7   r    ra   )r"   rY   r•   r�   r�   s        r#   Útest_coordinates_speeds_invalidz*TestSystem.test_coordinates_speeds_invalid²   sK   € ô �]‰]œ:Ó&ñ 	8Ø&ŒG�D—K‘K Ó&¨Ð7°Ò7÷	8à×!Ñ!Õ#÷	8ð 	8úó   šAÁAz)args, kwargs, exp_u_ind, exp_u_dep, exp_uc                 ó0  —  | j                   j                  |i |¤Ž | j                   j                  d d  |k(  sJ ‚| j                   j                  d d  |k(  sJ ‚| j                   j                  d d  |k(  sJ ‚| j                  d¬«       || j                   _        || j                   _        | j                   j                  d d  |k(  sJ ‚| j                   j                  d d  |k(  sJ ‚| j                   j                  d d  |k(  sJ ‚| j                  d¬«       y )N)r*   r+   r,   r„   )r    rJ   r*   r+   r,   r<   )r"   r$   r�   r�   Ú	exp_u_indÚ	exp_u_depÚexp_us          r#   Útest_speedszTestSystem.test_speedsÁ   s  € ð 	ˆ�‰×Ñ Ð/¨Ò/Ø�{‰{× Ñ ¡Ð# yÒ0Ð0Ð0Ø�{‰{× Ñ ¡Ð# yÒ0Ð0Ð0Ø�{‰{�}‰}™QÐ 5Ò(Ð(Ð(Ø× Ñ Ð)@Ð ÔAà%ˆ�‰ÔØ%ˆ�‰ÔØ�{‰{× Ñ ¡Ð# yÒ0Ð0Ð0Ø�{‰{× Ñ ¡Ð# yÒ0Ð0Ð0Ø�{‰{�}‰}™QÐ 5Ò(Ð(Ð(Ø× Ñ Ð)@Ð ÕAr%   zargs, kwargs, exp_u_auxc                 ó  —  | j                   j                  |i |¤Ž | j                   j                  d d  |k(  sJ ‚| j                  d¬«       || j                   _        | j                   j                  d d  |k(  sJ ‚| j                  d¬«       y )N)r-   r„   )r    rL   r-   r<   )r"   r$   r�   r�   Ú	exp_u_auxs        r#   Útest_auxiliary_speedsz TestSystem.test_auxiliary_speedsØ   s…   € ð 	)ˆ�‰×(Ñ(¨$Ð9°&Ò9Ø�{‰{× Ñ ¡Ð# yÒ0Ð0Ð0Ø× Ñ ¨Ð Ô4à%ˆ�‰ÔØ�{‰{× Ñ ¡Ð# yÒ0Ð0Ð0Ø× Ñ ¨Ð Õ4r%   c                 ó¸   — t        j                  t        «      5   | j                  j                  |i |¤Ž d d d «       | j                  «        y # 1 sw Y   ŒxY wrv   )rq   rŠ   r™   r    rL   ra   ©r"   rY   r�   r�   s       r#   Útest_auxiliary_invalidz!TestSystem.test_auxiliary_invalidæ   sJ   € ô �]‰]œ:Ó&ñ 	>Ø,ˆD�K‰K×,Ñ,¨dÐ=°fÒ=÷	>à×!Ñ!Õ#÷	>ð 	>úr›   zprop, add_func, args, kwargsr'   r(   r*   r+   r-   rL   r.   rK   r/   rN   r0   rO   r2   ri   rƒ   r1   Ú	add_loadsÚPÚNr4   Úadd_actuatorsÚTÚAc                 óX  — t        | j                  |d«       |ddf}|dv r|dz  }| j                  |¬«       t        t	        | j                  |«      d d  «      g k(  sJ ‚ t	        | j                  |«      |i |¤Ž t        t	        | j                  |«      d d  «      t        |«      k(  sJ ‚y )Nr5   r)   r,   )r/   r0   ©rQ   r„   )Úsetattrr    ra   Úlistr7   )r"   rY   ÚpropÚadd_funcr�   r�   r8   s          r#   Útest_add_after_resetzTestSystem.test_add_after_resetñ   s©   € ô$ 	�—‘˜T 2Ô&Ø˜˜cÐ"ˆØÐHÑHØÐ0Ñ0ˆGØ×!Ñ!¨'Ð!Ô2Ü”G˜DŸK™K¨Ó.©qÐ1Ó2°bÒ8Ð8Ð8Ø&Œ�—‘˜XÓ&¨Ð7°Ò7Ü”G˜DŸK™K¨Ó.©qÐ1Ó2´d¸4³jÒ@Ð@Ñ@r%   zprop, add_func, value, erroré   c                 ó2  — t        j                  |«      5   t        | j                  |«      |«       d d d «       t        j                  |«      5  t	        | j                  ||«       d d d «       | j                  «        y # 1 sw Y   ŒOxY w# 1 sw Y   Œ&xY wrv   )rq   rŠ   r7   r    r¯   ra   )r"   rY   r±   r²   ÚvalueÚerrors         r#   Útest_type_errorzTestSystem.test_type_error  sw   € ô  �]‰]˜5Ó!ñ 	2Ø*ŒG�D—K‘K Ó*¨5Ô1÷	2ä�]‰]˜5Ó!ñ 	.Ü�D—K‘K  uÔ-÷	.à×!Ñ!Õ#÷		2ð 	2ú÷	.ð 	.ús   –BÁBÂB
ÂBzargs, kwargs, exp_kdesr5   rF   é   c                 ó  —  | j                   j                  |i |¤Ž | j                  d¬«       | j                   j                  d d  |k(  sJ ‚|| j                   _        | j                  d¬«       | j                   j                  d d  |k(  sJ ‚y )N)r.   r„   )r    rK   ra   r.   )r"   rY   r�   r�   Úexp_kdess        r#   Ú	test_kdeszTestSystem.test_kdes"  s…   € ð 	ˆ�‰×Ñ˜dÐ- fÒ-Ø×!Ñ!¨)Ð!Ô4Ø�{‰{×Ñ¡Ð" hÒ.Ð.Ð.à#ˆ�‰ÔØ×!Ñ!¨)Ð!Ô4Ø�{‰{×Ñ¡Ð" hÒ.Ð.Ñ.r%   c                 ó¸   — t        j                  t        «      5   | j                  j                  |i |¤Ž d d d «       | j                  «        y # 1 sw Y   ŒxY wrv   )rq   rŠ   r™   r    rK   ra   r¥   s       r#   Útest_kdes_invalidzTestSystem.test_kdes_invalid1  sJ   € ô �]‰]œ:Ó&ñ 	2Ø ˆD�K‰K× Ñ  $Ð1¨&Ò1÷	2à×!Ñ!Õ#÷	2ð 	2úr›   zargs, kwargs, exp_conc                 ó  — d}|D �cg c]  }|j                  t        «      ‘Œ c}| j                  z   } | j                  j                  |i |¤Ž | j                  |¬«       | j                  j                  d d  |k(  sJ ‚| j                  j                  d d  |k(  sJ ‚|| j                  _        | j                  |¬«       | j                  j                  d d  |k(  sJ ‚| j                  j                  d d  |k(  sJ ‚y c c}w )N)r/   rQ   r„   )rg   rh   rS   r    rN   ra   r/   rQ   )r"   rY   r�   r�   Úexp_conr8   ÚcÚexp_vel_cons           r#   Útest_holonomic_constraintsz%TestSystem.test_holonomic_constraints;  së   € ð DˆØ*1Ö2 Q�q—v‘vœa•yÒ2°T·X±XÑ=ˆà-ˆ�‰×-Ñ-¨tÐ>°vÒ>Ø×!Ñ!¨'Ð!Ô2Ø�{‰{×0Ñ0±Ð3°wÒ>Ð>Ð>Ø�{‰{×/Ñ/±Ð2°kÒAÐAÐAà,3ˆ�‰Ô)Ø×!Ñ!¨'Ð!Ô2Ø�{‰{×0Ñ0±Ð3°wÒ>Ð>Ð>Ø�{‰{×/Ñ/±Ð2°kÒAÐAÑAùò 3s   ‡C=c                 ó¸   — t        j                  t        «      5   | j                  j                  |i |¤Ž d d d «       | j                  «        y # 1 sw Y   ŒxY wrv   )rq   rŠ   r™   r    rN   ra   r¥   s       r#   Ú"test_holonomic_constraints_invalidz-TestSystem.test_holonomic_constraints_invalidO  sN   € ô �]‰]œ:Ó&ñ 	CØ1ˆD�K‰K×1Ñ1°4ÐB¸6ÒB÷	Cà×!Ñ!Õ#÷	Cð 	Cúr›   c                 óä  — d}| j                   d t        | j                  «       |z   } | j                  j                  |i |¤Ž | j                  |¬«       | j                  j                  d d  |k(  sJ ‚| j                  j                  d d  |k(  sJ ‚|| j                  _        | j                  |¬«       | j                  j                  d d  |k(  sJ ‚| j                  j                  d d  |k(  sJ ‚y )N)r0   rQ   r„   )rR   ÚlenrP   r    rO   ra   r0   rQ   )r"   rY   r�   r�   rÀ   r8   rÂ   s          r#   Útest_nonholonomic_constraintsz(TestSystem.test_nonholonomic_constraintsZ  sâ   € ð GˆØ—g‘g˜mœs 4§7¡7›|Ð,¨wÑ6ˆà0ˆ�‰×0Ñ0°$ÐA¸&ÒAØ×!Ñ!¨'Ð!Ô2Ø�{‰{×3Ñ3±AÐ6¸'ÒAÐAÐAØ�{‰{×/Ñ/±Ð2°kÒAÐAÐAà/6ˆ�‰Ô,Ø×!Ñ!¨'Ð!Ô2Ø�{‰{×3Ñ3±AÐ6¸'ÒAÐAÐAØ�{‰{×/Ñ/±Ð2°kÒAÐAÑAr%   c                 ó¸   — t        j                  t        «      5   | j                  j                  |i |¤Ž d d d «       | j                  «        y # 1 sw Y   ŒxY wrv   )rq   rŠ   r™   r    rO   ra   r¥   s       r#   Ú%test_nonholonomic_constraints_invalidz0TestSystem.test_nonholonomic_constraints_invalidn  sN   € ô �]‰]œ:Ó&ñ 	FØ4ˆD�K‰K×4Ñ4°dÐE¸fÒE÷	Fà×!Ñ!Õ#÷	Fð 	Fúr›   zconstraints, expectedc                 ó†   — || j                   _        | j                  d¬«       | j                   j                  d d  |k(  sJ ‚y )Nr®   r„   )r    rQ   ra   )r"   rY   ÚconstraintsÚexpecteds       r#   Ú#test_velocity_constraints_overwritez.TestSystem.test_velocity_constraints_overwritez  s?   € ð ,7ˆ�‰Ô(Ø×!Ñ!Ð*CÐ!ÔDØ�{‰{×/Ñ/±Ð2°hÒ>Ð>Ñ>r%   c                 ó  — t         d   t         d   z
  | j                  _        | j                  d¬«       | j                  j                  d d  t         d   t         d   z
  gk(  sJ ‚d | j                  _        | j                  «        y )NrD   rB   r®   r„   )rG   r    rQ   ra   rz   s     r#   Ú&test_velocity_constraints_back_to_autoz1TestSystem.test_velocity_constraints_back_to_auto…  so   € Ü+-¨a©5´2°a±5©=ˆ�‰Ô(Ø×!Ñ!Ð*CÐ!ÔDØ�{‰{×/Ñ/±Ð2´r¸!±u¼rÀ!¹u±}°oÒEÐEÐEØ+/ˆ�‰Ô(Ø×!Ñ!Õ#r%   c                 ó†  ‡ ‡‡— t        d«      t        d«      }}t        d«      t        d«      c}Š‰ j                  j                  ||«       ‰ j                  j                  g ‰ j                  ¢|‘|‘­k(  sJ ‚‰ j                  j                  ‰«       ‰ j                  j                  g ‰ j                  ¢|‘|‘‰‘­k(  sJ ‚g ‰ j                  _        ‰ j                  j                  dk(  sJ ‚‰‰ j                  _        ‰ j                  j                  ‰fk(  sJ ‚t        d«      Št        j                  t        ˆ ˆfd„«       t        j                  t        ˆˆ fd„«       t        j                  t        «      5  |||‰‰f‰ j                  _        d d d «       ‰ j                  j                  ‰fk(  sJ ‚y # 1 sw Y   Œ&xY w)	NÚrb1Úrb2Úp1Úp2r5   Úsymbc                  ó:   •— ‰ j                   j                  ‰«      S rv   ©r    ri   )r"   rÖ   s   €€r#   ú<lambda>z(TestSystem.test_bodies.<locals>.<lambda>˜  s   ø€ ¨¯©×)?Ñ)?ÀÓ)E€ r%   c                  ó:   •— ‰j                   j                  ‰ «      S rv   rØ   )rÕ   r"   s   €€r#   rÙ   z(TestSystem.test_bodies.<locals>.<lambda>™  s   ø€ ¨$¯+©+×*@Ñ*@ÀÓ*D€ r%   )
r   r   r    ri   r2   r   rq   rŠ   r‹   r™   )r"   rY   rÒ   rÓ   rÔ   rÕ   rÖ   s   `    @@r#   Útest_bodieszTestSystem.test_bodiesŒ  so  ú€ Ü˜UÓ#¤Y¨uÓ%5ˆSˆÜ˜$“¤¨$£ˆˆˆBØ�‰×Ñ˜s BÔ'Ø�{‰{×!Ñ!Ð%< t§{¡{Ð%<°CÐ%<¸Ñ%<Ò<Ð<Ð<Ø�‰×Ñ˜rÔ"Ø�{‰{×!Ñ!Ð%@ t§{¡{Ð%@°CÐ%@¸Ð%@¸RÑ%@Ò@Ð@Ð@Øˆ�‰ÔØ�{‰{×!Ñ! RÒ'Ð'Ð'Øˆ�‰ÔØ�{‰{×!Ñ! b UÒ*Ð*Ð*Ü�v‹ˆÜ�‰”iÔ!EÔFÜ�‰”jÔ"DÔEÜ�]‰]œ9Ó%ñ 	:Ø"% s¨B°°DÐ!9ˆD�K‰KÔ÷	:à�{‰{×!Ñ! b UÒ*Ð*Ñ*÷	:ð 	:ús   Å;F7Æ7G c                 óî  ‡‡— t        «       Št        d«      t        d«      cŠ}t        d‰¬«      }t        d«      }t	        d|«      }‰j                  t        |‰j                  «      ||j                  ft        ||j                  «      «       ‰j                  ‰‰j                  f||j                  f||j                  ffk(  sJ ‚||j                  fg‰_	        ‰j                  ||j                  ffk(  sJ ‚t        j                  t        ˆˆfd„«       t        j                  t        «      5  ‰‰j                  f‰_	        d d d «       ‰j                  ||j                  ffk(  sJ ‚y # 1 sw Y   Œ(xY w)Nr©   r¬   rÒ   )r   Úmc1rÔ   c                  óT   •— ‰j                  ‰ ‰ j                  ‰ j                  f«      S rv   )r§   rl   Úy)r©   r    s   €€r#   rÙ   z+TestSystem.test_add_loads.<locals>.<lambda>¨  s    ø€ ¨&×*:Ñ*:¸A¸q¿s¹sÀAÇCÁC¸=Ó*I€ r%   )r   r   r   r   r   r§   r   rl   r
   r1   rq   rŠ   r™   r‹   )r"   r¬   rÒ   rÝ   rÔ   r©   r    s        @@r#   Útest_add_loadszTestSystem.test_add_loadsž  s(  ù€ Ü“ˆÜ˜cÓ"¤N°3Ó$7ˆˆˆ1Ü˜ QÔ'ˆÜ�E‹lˆÜ�d˜CÓ ˆØ×Ñœ  Q§S¡SÓ)¨C°·±¨:´u¸RÀÇÁ³~ÔFØ�|‰|  A§C¡C ¨3°·±¨*°s¸A¿C¹C°jÐAÒAÐAÐAØ˜AŸC™C˜�zˆŒØ�|‰|  A§C¡C ˜{Ò*Ð*Ð*Ü�‰”jÔ"IÔJÜ�]‰]œ9Ó%ñ 	$Ø˜qŸs™s˜8ˆFŒL÷	$à�|‰|  A§C¡C ˜{Ò*Ð*Ñ*÷	$ð 	$ús   Ä0E+Å+E4c                 ób  — t        «       }t        d«      t        d«      }}t        t        d«      |j                  |«      }t        t        d«      |j
                  ||«      }|j                  |«       |j                  |fk(  sJ ‚|j                  dk(  sJ ‚|f|_        |j                  |fk(  sJ ‚y )Nr©   r¬   ÚT1ÚT2r5   )	r   r   r   r   rl   rß   rª   r4   r1   )r"   r    r©   r¬   Úact1Úact2s         r#   Útest_add_actuatorszTestSystem.test_add_actuators­  s    € Ü“ˆÜ˜cÓ"¤N°3Ó$7ˆ1ˆÜœg d›m¨Q¯S©S°!Ó4ˆÜœg d›m¨Q¯S©S°!°QÓ7ˆØ×Ñ˜TÔ"Ø×Ñ D 7Ò*Ð*Ð*Ø�|‰|˜rÒ!Ð!Ð!Ø ˜7ˆÔØ×Ñ D 7Ò*Ð*Ñ*r%   c           	      óä  ‡‡‡— t        d«      \  }}}}}}}t        dt        ¬«      \  Š}}	}
}t        d‰|||«      }t	        d||	||«      Št        d|	|
||«      }t        d|
|||j                  t        «      «      }t        «       Š‰j                  |«       ‰j                  |fk(  sJ ‚‰j                  ‰|fk(  sJ ‚‰j                  t        |g«      k(  sJ ‚‰j                  t        |g«      k(  sJ ‚‰j                  t        ||j                  t        «      z
  g«      k(  sJ ‚‰j                  |
«       ‰j!                  |«       ‰j#                  ||j                  t        «      z
  «       ‰j                  |«       ‰j                  ||fk(  sJ ‚‰j                  ‰||
|	fk(  sJ ‚‰j                  t        ||g«      k(  sJ ‚‰j                  t        ||g«      k(  sJ ‚‰j                  t        ||j                  t        «      z
  ||j                  t        «      z
  g«      k(  sJ ‚‰j#                  ||j                  t        «      z
   «       ‰j                  ‰«       ‰j                  ||‰fk(  sJ ‚‰j                  ‰||
|	fk(  sJ ‚‰j                  t        |||g«      k(  sJ ‚‰j                  t        |||g«      k(  sJ ‚‰j                  t        ||j                  t        «      z
  ||j                  t        «      z
  ||j                  t        «      z
   g«      k(  sJ ‚‰j                  |«       ‰j                  ||‰|fk(  sJ ‚‰j                  ‰||
|	|fk(  sJ ‚‰j                  t        ||||g«      k(  sJ ‚‰j                  t        ||||j                  t        «      g«      k(  sJ ‚‰j                  t        ||j                  t        «      z
  ||j                  t        «      z
  ||j                  t        «      z
   g«      k(  sJ ‚‰j$                  d d  g k(  sJ ‚‰j&                  d d  g k(  sJ ‚t)        j*                  t,        ˆˆfd„«       t)        j*                  t.        ˆˆfd	„«       y )
Nz	q1:5 u1:4zrb1:6r>   r@   rA   rC   ÚJ_lagc                  ó&   •— ‰j                  ‰ «      S rv   ©rH   )rA   r    s   €€r#   rÙ   z,TestSystem.test_add_joints.<locals>.<lambda>á  s   ø€ ¨&×*;Ñ*;¸BÓ*?€ r%   c                  ó&   •— ‰j                  ‰ «      S rv   rê   )rÒ   r    s   €€r#   rÙ   z,TestSystem.test_add_joints.<locals>.<lambda>â  s   ø€ ¨×):Ñ):¸3Ó)?€ r%   )r   r   r   r   r   rg   rh   r   rH   r3   r2   r'   r	   r*   r.   ri   rI   rK   r(   r+   rq   rŠ   r™   r‹   )r"   Úq1Úq2Úq3Úq4Úu1Úu2Úu3rÓ   Úrb3Úrb4Úrb5r@   rC   rè   rA   rÒ   r    s                  @@@r#   Útest_add_jointszTestSystem.test_add_joints¸  s  ú€ Ü%3°KÓ%@Ñ"ˆˆB��B˜˜B Ü")¨'´yÔ"AÑˆˆS�#�s˜CÜ�d˜C  b¨"Ó-ˆÜ˜D # s¨B°Ó3ˆÜ�d˜C  b¨"Ó-ˆÜ˜ # s¨B°·±¼³
Ó;ˆÜ“ˆØ×Ñ˜"ÔØ�}‰}  Ò%Ð%Ð%Ø�}‰}  c 
Ò*Ð*Ð*Ø�|‰|œ°¨tÓ4Ò4Ð4Ð4Ø�|‰|œ°¨tÓ4Ò4Ð4Ð4Ø�{‰{œo¨r°B·G±G¼A³J©Ð.?Ó@Ò@Ð@Ð@Ø×Ñ˜#ÔØ×Ñ˜rÔ"Ø�‰˜˜RŸW™W¤Q›Z™Ô(Ø×Ñ˜"ÔØ�}‰}  R Ò(Ð(Ð(Ø�}‰}  c¨3°Ð 4Ò4Ð4Ð4Ø�|‰|œ°°B¨xÓ8Ò8Ð8Ð8Ø�|‰|œ°°B¨xÓ8Ò8Ð8Ð8Ø�{‰{œoØ�"—'‘'œ!“*‰_˜b 2§7¡7¬1£:™oÐ.ó0ò 0ð 	0ð 0à�‰˜"˜rŸw™w¤q›z™/Ð*Ô+Ø×Ñ˜"ÔØ�}‰}  R¨ Ò,Ð,Ð,Ø�}‰}  c¨3°Ð 4Ò4Ð4Ð4Ø�|‰|œ°°B¸¨|Ó<Ò<Ð<Ð<Ø�|‰|œ°°B¸¨|Ó<Ò<Ð<Ð<Ø�{‰{œo¨r°B·G±G¼A³J©ÀÀRÇWÁWÌQÃZÁØ13°b·g±g¼a³j±Ð/Að/Có Dò Dð 	Dð Dà×Ñ˜%Ô Ø�}‰}  R¨¨UÐ 3Ò3Ð3Ð3Ø�}‰}  c¨3°°SÐ 9Ò9Ð9Ð9Ø�|‰|œ°°B¸¸BÐ/?Ó@Ò@Ð@Ð@Ø�|‰|œ°°B¸¸B¿G¹GÄA»JÐ/GÓHÒHÐHÐHØ�{‰{œo¨r°B·G±G¼A³J©ÀÀRÇWÁWÌQÃZÁØ13°b·g±g¼a³j±Ð/Að/Có Dò Dð 	Dð Dà�|‰|™Aˆ "Ò$Ð$Ð$Ø�|‰|™Aˆ "Ò$Ð$Ð$Ü�‰”jÔ"?Ô@Ü�‰”iÔ!?Õ@r%   c                 óN  — | j                   dd  | j                  _         | j                  j                   | j                   dd  k(  sJ ‚| j                  d¬«       d| j                  _        d| j                  _        | j                   | j                  _         | j                  «        y )Nr   )r3   r„   r5   )r3   r    ra   r'   r*   rz   s     r#   Útest_joints_setterzTestSystem.test_joints_setterä  s   € Ø!Ÿ[™[¨¨˜_ˆ�‰ÔØ�{‰{×!Ñ! T§[¡[°° _Ò4Ð4Ð4Ø×!Ñ!¨+Ð!Ô6Øˆ�‰ÔØˆ�‰ÔØ!Ÿ[™[ˆ�‰ÔØ×!Ñ!Õ#r%   zname, joint_index))r@   r   )rA   r   ©Únot_existingNc                 óp   — | j                   j                  |«      }|€|�J ‚y || j                  |   k(  sJ ‚y rv   )r    Ú	get_jointr3   )r"   rY   r   Újoint_indexÚjoints        r#   Útest_get_jointzTestSystem.test_get_jointí  s?   € ð —‘×%Ñ% dÓ+ˆØÐØ�=Ð ‘=à˜DŸK™K¨Ñ4Ò4Ð4Ñ4r%   zname, body_index))rÒ   r   )ró   rB   rù   c                 óp   — | j                   j                  |«      }|€|�J ‚y || j                  |   k(  sJ ‚y rv   )r    Úget_bodyr2   )r"   rY   r   Ú
body_indexrƒ   s        r#   Útest_get_bodyzTestSystem.test_get_bodyù  s?   € ð �{‰{×#Ñ# DÓ)ˆØÐØ�<Ð‘<à˜4Ÿ;™; zÑ2Ò2Ð2Ñ2r%   r6   c                 ó–   —  G d„ d|«      }| j                   j                  |¬«       t        | j                   j                  |«      sJ ‚y )Nc                   ó   — e Zd Zy)ú:TestSystem.test_form_eoms_calls_subclass.<locals>.MyMethodN)rn   ro   rp   r5   r%   r#   ÚMyMethodr    s   „ Ør%   r  )r6   )r    Ú	form_eomsr€   r6   )r"   rm   r6   r  s       r#   Útest_form_eoms_calls_subclassz(TestSystem.test_form_eoms_calls_subclass  s?   € ô	�zô 	ð 	�‰×Ñ¨ÐÔ2Ü˜$Ÿ+™+×0Ñ0°(Ô;Ð;Ñ;r%   zkwargs, expectedéÿÿÿÿrd   Úexplicit_kinematicsc                 ór   —  | j                   j                  di |¤Ž | j                   j                  |k(  sJ ‚y )Nr5   )r    r  Úmass_matrix_full)r"   rm   r�   rÍ   s       r#   Ú!test_system_kane_form_eoms_kwargsz,TestSystem.test_system_kane_form_eoms_kwargs  s3   € ð 	ˆ�‰×ÑÑ' Ò'Ø�{‰{×+Ñ+¨xÒ7Ð7Ñ7r%   zkwargs, mm, gmc                 ó´   —  | j                   j                  ddt        i|¤Ž | j                   j                  |k(  sJ ‚| j                   j                  |k(  sJ ‚y )Nr6   r5   )r    r  r   r  Úforcing_full)r"   rm   r�   ÚmmÚgms        r#   Ú%test_system_lagrange_form_eoms_kwargsz0TestSystem.test_system_lagrange_form_eoms_kwargs  sP   € ð 	ˆ�‰×ÑÑC¬ÐC¸FÒCØ�{‰{×+Ñ+¨rÒ1Ð1Ð1Ø�{‰{×'Ñ'¨2Ò-Ð-Ñ-r%   zeom_method, kwargs, errorÚnon_existing_kwargÚkd_eqsÚ
Lagrangianc                 óÂ  — t         d   | j                  _        t        dt	        d«      ¬«      }| j                  j                  |«       |j                  j                  | j                  j                  t         d   | j                  j                  z  «       t        j                  |«      5   | j                  j                  dd|i|¤Ž d d d «       y # 1 sw Y   y xY w)Nr   rc   rd   re   r6   r5   )r)   r    r'   r   r   ri   rj   rk   r   rl   rq   rŠ   r  )r"   r$   r6   r�   r·   rc   s         r#   Útest_form_eoms_kwargs_errorsz'TestSystem.test_form_eoms_kwargs_errors!  s¦   € ô ˜a™Dˆ�‰ÔÜ�Sœw s›|Ô,ˆØ�‰×Ñ˜qÔ!Ø	�‰×Ñ˜TŸ[™[×4Ñ4´a¸±d¸T¿[¹[¿]¹]Ñ6JÔKÜ�]‰]˜5Ó!ñ 	CØ!ˆD�K‰K×!Ñ!ÑB¨ZÐB¸6ÒB÷	C÷ 	Cñ 	Cús   Â-CÃC)8rn   ro   rp   rx   r{   rq   ÚmarkÚparametrizer   r   r�   r‡   r�   r)   r”   r,   r   rš   r    rM   r£   r¦   rG   r   r
   rl   r   r³   r™   r‹   r¸   r]   r¼   r¾   rÃ   rÅ   rÈ   rÊ   rÎ   rÐ   rÛ   rà   ræ   rö   rø   rÿ   r  r   r   r	  r	   r  rg   rh   r  r  )Ú.0r_   r`   s   000r#   rt   rt   k   s«  „ ò&ò&ð ‡[�[×Ñ˜W t©^¸GÓ-DÐ&EÓFØ‡[�[×Ñ˜]¨T±5¸Ó3GÐ,HÓIñQó Jó GðQò0#ò&ð
 ‡[�[×ÑÐHØ	
ˆ2ˆAˆ��A�b�q�E˜2˜q  !˜uÐ%Ø	
ˆ2ˆAˆ� Ð% q¨¨! u¨b°!°B°Q°%Ð8Ø	
ˆ2ˆAˆ� Ð&¨¨A¨b¨q¨E°1°R°a°5Ð9Ø	
ˆ2ˆAˆ�Ò 3Ð4°q¸±t¸Q¸q¹T°lÀQÀqÁTÀFØ
ˆA‰$��!‘�a˜‘dÐ	ð	ð	Kó ñBóðBð  ‡[�[×Ñ˜VÐ&7¸Ð%FÓGØ‡[�[×Ñ˜^Ø
ˆA‰$��!‘ˆ�rÐØ
ˆA‰$��!‘ˆ�rÐØ
ˆA‰$ˆ�= %Ð(Ð)Ø
ˆA‰$ˆ�= %Ð(Ð)Ø
ˆA‰$��!‘ˆ�rÐÙ
�#‹,˜˜!™Ð	˜rÐ"ð.ó ñ$óó Hð$ð ‡[�[×ÑÐHØ	
ˆ2ˆAˆ��A�b�q�E˜2˜q  !˜uÐ%Ø	
ˆ2ˆAˆ� Ð% q¨¨! u¨b°!°B°Q°%Ð8Ø	
ˆ2ˆAˆ� Ð&¨¨A¨b¨q¨E°1°R°a°5Ð9Ø	
ˆ2ˆAˆ�Ò 3Ð4°q¸±t¸Q¸q¹T°lÀQÀqÁTÀFØ
ˆA‰$��!‘�a˜‘dÐ	ð	ð	Kó ñBóðBð  ‡[�[×ÑÐ6Ø	ˆBˆQˆ��R˜˜�VÐð9ó ñ	5óð	5ð ‡[�[×Ñ˜^Ø
ˆQ‰%��1‘ˆ˜ÐØ
ˆQ‰%��1‘ˆ˜ÐØ
ˆQ‰%��A‘ˆ˜ÐÙ
�#‹,˜˜1™Ð	 Ð#ð	.ó ñ$óð$ð
 ‡[�[×ÑÐ;Ø	Ð# a¨¡d W¨bÐ1Ø	Ð# a¨¡d W¨}¸eÐ.DÐEØ	�,  1¡ ¨Ð,Ø	�,  1¡ ¨-¸Ð)?Ð@Ø	Ð(¨2¨a©5¨(°BÐ7Ø	�˜b ™e a¨¡d™l˜_¨bÐ1Ø	 Ð"=Ø
ˆA‰$��1‘‰+ˆ˜ð	à	#Ð%CØ
ˆA‰$��1‘‰+ˆ˜ð	à	�<¡)¨FÓ"3Ð!5°rÐ:Ø	�+¡¡e¨C£j±.ÀÓ2E×2GÑ2GÓ HÐJÈBÐOØ	�o©Ù�C‹L™.¨Ó-×/Ñ/±ÀÓ1Dó)Fð (HØIKð	Mð>ó ñ 	Aó!ð 	Að ‡[�[×ÑÐ;Ø	Ð#¡W¨S£\°:Ð>Ø	Ð#¡W¨S£\°:Ð>Ø	�,¡¨£¨jÐ9Ø	�,¡¨£¨jÐ9Ø	Ð(©'°#«,¸
ÐCØ	�˜Q 	Ð*Ø	 Ð"=¸qÀ)ÐLØ	#Ð%CÀQØ	ð	à	�<¡¨£¨yÐ9Ø	�+™w s›|¨YÐ7Ø	�o¡w¨s£|°YÐ?ð>ó ñ$óð$ð ‡[�[×ÑÐ5Ø	ˆR©#¨a°°¨e°R¸¸°VÓ*<×=Ð=™w˜r 3�"�s“(Ô=Ð>Ø
ˆA‰$��A‘‰,˜˜!™˜r !™u™Ð	% rÙ"% a¨¨ e¨R°°¨VÓ"4×	5Ð	5‘w�r˜3ˆ"ˆs‹(Ô	5ð	7ð8ó ñ
/óð
/ð ‡[�[×Ñ˜^Ø
ˆA‰$��A‘‰,˜˜!™˜r !™u™Ð	% rÐ*Øˆa‰D�2�a‘5‰Lˆ/˜1˜Q™4 " Q¡%™<Ð	(¨"Ð-ØˆQ‰4�!�A‘$‰;˜˜!™˜r !™u™Ð
%¨Ð+ð.ó ñ
$óð
$ð
 ‡[�[×ÑÐ4Ø	ˆR�!�A‘$˜˜1™‘+  !¡Ñ$Ð%Ð&Ø
ˆA‰$��1‘‰+�q˜‘t˜a ™d‘{Ð	# RØ
ˆA‰$��1‘‰+˜˜!™Ñ
˜a ™d Q q¡T™k¨1¨Q©4°!°A±$©;Ð	7ð	9ð7ó ñ
Bóð
Bð ‡[�[×Ñ˜^Ø
ˆA‰$��1‘‰+˜˜!™Ñ
˜a ™d Q q¡T™kÐ	*¨BÐ/Øˆa‰D�1�Q‘4‰K˜!˜A™$ÑÐ
  1¡¨¨!©¡Ð	-¨rÐ2Ø
ˆA‰$��1‘‰+�q˜‘t˜a ™d‘{Ð	# RÐ(ð.ó ñ
$óð
$ð ‡[�[×ÑÐ4Ø	ˆR�!�A‘$˜˜A™‘,  1¡Ñ%Ð&Ð'Ø
ˆA‰$��1‘‰+�q˜‘t˜a ™d‘{Ð	# RØ
ˆA‰$��A‘‰,˜˜1™Ñ
˜q ™t a¨¡d™{¨A¨a©D°1°Q±4©KÐ	8ð	:ð7ó ñ
Bóð
Bð ‡[�[×Ñ˜^Ø
ˆA‰$��A‘‰,˜˜1™Ñ
˜q ™t a¨¡d™{Ð	+¨RÐ0Øˆa‰D�2�a‘5‰L˜1˜Q™4ÑÐ
  ! A¡$¨¨1©¡+Ð	.°Ð3Ø
ˆA‰$��1‘‰+�q˜‘t˜a ™d‘{Ð	# RÐ(ØˆQ‰4�!�A‘$‰;˜˜!™˜q ™t™Ð
$ rÐ*ð	.ó ñ$óð$ð ‡[�[×ÑÐ4Ø	ˆRˆØ	ˆA‰��A‘‰˜˜A™Ñ	  A¡¨¨A©¡°°A±Ñ!6Ð 7Ð8Ø
ˆQ‰%�"�Q‘%‰-˜˜1™  !¡™Ð	%¨¨1©°°1±©°q¸±t¸aÀ¹d±{Ð'CÐDð7ó ñ
?óð
?ò$ò+ò$+ò	+ò*AòX$ð ‡[�[×ÑÐ0ò 3ó ñ
5óð
5ð ‡[�[×ÑÐ/ò 2ó ñ
3óð
3ð ‡[�[×Ñ˜\¨K¸Ð+IÓJñ<ó Kð<ð ‡[�[×ÑÐ/Ø	‰_˜r 1˜g¨©7°3«<Ð'8Ð9Ó:Ð;Ø
 Ð	&©¸!¸Q¸Ø:;¹WÀS»\Ð9Jð9Ló )Mð 	Nð2ó ñ
8óð
8ð
 ‡[�[×ÑÐ-Ø	‰_˜q !˜f q©'°#«,Ð&7Ð8Ó9Ù	˜!˜A™$Ÿ)™) A›,¨Ð*Ó	+ð	-ð0ó ñ.ó	ð.ð ‡[�[×ÑÐ8Ø	Ð+¨QÐ/°Ð;Ø	Ð/°Ð3°YÐ?Ø	�x �n jÐ1Ø	�x �n jÐ1Ø	˜8 R˜.¨*Ð5Ø	˜<¨Ð+¨ZÐ8ð;ó ñCóñCùôM >ùä	5s   Ï aÐa&rt   c                   ó0  — e Zd Zej                  j                  deedfeedfg«      d„ «       Zej                  j                  dedfedfg«      d„ «       Z	d„ Z
d„ Zd	„ Zej                  j                  dedfedfg«      d
„ «       Zd„ Zd„ Zd„ Zd„ Zd„ Zy)ÚTestValidateSystemz)valid_method, invalid_method, with_speedsTFc                 óð   — | j                  |¬«       | j                  j                  |«       t        j                  t
        «      5  | j                  j                  |«       d d d «       y # 1 sw Y   y xY w)NrW   )rU   r    rw   rq   rŠ   r™   )r"   Úvalid_methodÚinvalid_methodrT   s       r#   Útest_only_validz"TestValidateSystem.test_only_valid4  sZ   € ð
 	×"Ñ"¨{Ð"Ô;Ø�‰×#Ñ# LÔ1ä�]‰]œ:Ó&ñ 	8Ø�K‰K×'Ñ'¨Ô7÷	8÷ 	8ñ 	8ús   ÁA,Á,A5zmethod, with_speedsc                 ó  ‡ ‡— ‰ j                  |¬«       ‰ j                  dd  ‰ j                  _        ‰ j                  d d ‰ j                  _        ‰ j                  d d ‰ j                  _        t        j                  t        ˆˆ fd„«       y )NrW   r   r
  c                  ó:   •— ‰j                   j                  ‰ «      S rv   ©r    rw   ©Úmethodr"   s   €€r#   rÙ   zBTestValidateSystem.test_missing_joint_coordinate.<locals>.<lambda>F  ó   ø€ ¨$¯+©+×*EÑ*EÀfÓ*M€ r%   )rU   r'   r    r*   r.   rq   rŠ   r™   ©r"   r&  rT   s   `` r#   Útest_missing_joint_coordinatez0TestValidateSystem.test_missing_joint_coordinate?  sg   ù€ ð 	×"Ñ"¨{Ð"Ô;Ø ŸJ™J q r˜Nˆ�‰ÔØ ŸJ™J s¨˜Oˆ�‰ÔØŸ9™9 S b˜>ˆ�‰ÔÜ�‰”jÔ"MÕNr%   c                 óô   ‡ — ‰ j                   d d ‰ j                  _         ‰ j                  dd  ‰ j                  _        ‰ j                  d d ‰ j                  _        t	        j
                  t        ˆ fd„«       y )Nr
  r   c                  ó8   •— ‰ j                   j                  «       S rv   r$  r!   s   €r#   rÙ   z=TestValidateSystem.test_missing_joint_speed.<locals>.<lambda>L  ó   ø€ ¨$¯+©+×*EÑ*EÓ*G€ r%   )r'   r    r*   r.   rq   rŠ   r™   rz   s   ` r#   Útest_missing_joint_speedz+TestValidateSystem.test_missing_joint_speedH  sT   ø€ Ø ŸJ™J s¨˜Oˆ�‰ÔØ ŸJ™J q r˜Nˆ�‰ÔØŸ9™9 S b˜>ˆ�‰ÔÜ�‰”jÓ"GÕHr%   c                 ó|   ‡ — ‰ j                   dd  ‰ j                  _         t        j                  t        ˆ fd„«       y )Nr   c                  ó8   •— ‰ j                   j                  «       S rv   r$  r!   s   €r#   rÙ   z<TestValidateSystem.test_missing_joint_kdes.<locals>.<lambda>P  r,  r%   )r.   r    rq   rŠ   r™   rz   s   ` r#   Útest_missing_joint_kdesz*TestValidateSystem.test_missing_joint_kdesN  s(   ø€ ØŸ9™9 Q R˜=ˆ�‰ÔÜ�‰”jÓ"GÕHr%   c                 ó˜   — | j                   d    g| j                   dd  z   | j                  _         | j                  j                  «        y )Nr   r   )r.   r    rw   rz   s     r#   Útest_negative_joint_kdesz+TestValidateSystem.test_negative_joint_kdesR  s9   € Ø!ŸY™Y q™\˜M˜?¨T¯Y©Y°q°r¨]Ñ:ˆ�‰ÔØ�‰×#Ñ#Õ%r%   c                 óÊ  ‡ ‡— ‰ j                  |¬«       g ‰ j                  _        ‰ j                  ‰ j                  d   ‰ j
                  d   z
  ‰ j                  d   z   gz   ‰ j                  _        t        j                  t        ˆˆ fd„«       g ‰ j                  _
        ‰ j                  ‰ j                  z   ‰ j                  _        ‰ j                  j                  ‰«       y )NrW   r   r   c                  ó:   •— ‰j                   j                  ‰ «      S rv   r$  r%  s   €€r#   rÙ   zFTestValidateSystem.test_missing_holonomic_constraint.<locals>.<lambda>]  r'  r%   )rU   r    r/   rS   r*   r+   r0   rq   rŠ   r™   r(   r'   rw   r(  s   `` r#   Ú!test_missing_holonomic_constraintz4TestValidateSystem.test_missing_holonomic_constraintV  s§   ù€ ð 	×"Ñ"¨{Ð"Ô;Ø,.ˆ�‰Ô)Ø/3¯x©xØ�J‰J�q‰M˜DŸJ™J q™MÑ)¨D¯J©J°q©MÑ9ð;;ñ 0;ˆ�‰Ô,ä�‰”jÔ"MÔNØˆ�‰ÔØ ŸJ™J¨¯©Ñ3ˆ�‰ÔØ�‰×#Ñ# FÕ+r%   c                 ó*  ‡ — g ‰ j                   _        t        j                  t        ˆ fd„«       ‰ j
                  d   ‰ j                   _        ‰ j                  ‰ j
                  d   gz   ‰ j                   _        ‰ j                   j                  «        y )Nc                  ó8   •— ‰ j                   j                  «       S rv   r$  r!   s   €r#   rÙ   zITestValidateSystem.test_missing_nonholonomic_constraint.<locals>.<lambda>d  r,  r%   r   r   )r    r0   rq   rŠ   r™   r+   r*   rw   rz   s   ` r#   Ú$test_missing_nonholonomic_constraintz7TestValidateSystem.test_missing_nonholonomic_constraintb  sa   ø€ Ø/1ˆ�‰Ô,Ü�‰”jÓ"GÔHØ ŸJ™J q™Mˆ�‰ÔØ ŸJ™J¨$¯*©*°Q©-¨Ñ8ˆ�‰ÔØ�‰×#Ñ#Õ%r%   c                 óÎ  ‡ — ‰ j                   t        d   gz   ‰ j                  _         ‰ j                  t        d   t        d   z
  gz   ‰ j                  _        ‰ j                  j                  «        ‰ j                  ‰ j                  _        ‰ j                   d d ‰ j                  _         ‰ j                  d d ‰ j                  _        t        j                  t        ˆ fd„«       y )Nr–   r
  c                  ó8   •— ‰ j                   j                  «       S rv   r$  r!   s   €r#   rÙ   zFTestValidateSystem.test_number_of_coordinates_speeds.<locals>.<lambda>r  r,  r%   )
r*   r,   r    r.   rG   rw   r'   rq   rŠ   r™   rz   s   ` r#   Ú!test_number_of_coordinates_speedsz4TestValidateSystem.test_number_of_coordinates_speedsi  sœ   ø€ à ŸJ™J¬!¨A©$¨Ñ/ˆ�‰ÔØŸ9™9¬¨!©¬r°!©u© ~Ñ5ˆ�‰ÔØ�‰×#Ñ#Ô%à ŸJ™Jˆ�‰ÔØ ŸJ™J s¨˜Oˆ�‰ÔØŸ9™9 S b˜>ˆ�‰ÔÜ�‰”jÓ"GÕHr%   c                 ó*  ‡ — ‰ j                   d d ‰ j                  _         t        j                  t        ˆ fd„«       ‰ j                   t
        d   t
        d   z   t        d   z
  gz   ‰ j                  _         t        j                  t        ˆ fd„«       y )Nr
  c                  ó8   •— ‰ j                   j                  «       S rv   r$  r!   s   €r#   rÙ   z8TestValidateSystem.test_number_of_kdes.<locals>.<lambda>w  r,  r%   rB   r   c                  ó8   •— ‰ j                   j                  «       S rv   r$  r!   s   €r#   rÙ   z8TestValidateSystem.test_number_of_kdes.<locals>.<lambda>y  r,  r%   )r.   r    rq   rŠ   r™   r,   rG   rz   s   ` r#   Útest_number_of_kdesz&TestValidateSystem.test_number_of_kdest  sd   ø€ àŸ9™9 S b˜>ˆ�‰ÔÜ�‰”jÓ"GÔHØŸ9™9¬¨!©¬q°©t©´b¸±eÑ(;Ð'<Ñ<ˆ�‰ÔÜ�‰”jÓ"GÕHr%   c                 ó<   — | j                   j                  d¬«       y )NT)Úcheck_duplicatesr$  rz   s     r#   Útest_duplicatesz"TestValidateSystem.test_duplicates{  s   € à�‰×#Ñ#°TÐ#Õ:r%   c           	      ó¦  — t         d t        | j                  «       | j                  _        t	        j
                  t        «      5  | j                  j                  t        «       d d d «       g | j                  _        | j                  j                  t        «       t        | j                  _
        t	        j
                  t        «      5  | j                  j                  t        «       d d d «       g | j                  _
        | j                  j                  t        «       | j                  j                  t        dt        d«      t        d«      «      «       g | j                  _        t	        j
                  t        «      5  | j                  j                  t        «       d d d «       y # 1 sw Y   �ŒKxY w# 1 sw Y   ŒÑxY w# 1 sw Y   y xY w)NÚJuÚrbu1Úrbu2)r,   rÇ   r*   r    rq   rŠ   r™   rw   r   rM   r-   rH   r   r   )r"   r[   s     r#   Útest_speeds_in_lagrangez*TestValidateSystem.test_speeds_in_lagrange  s9  € ÜÐ.œs 4§:¡:›Ð/ˆ�‰ÔÜ�]‰]œ:Ó&ñ 	9Ø�K‰K×'Ñ'¬Ô8÷	9àˆ�‰ÔØ�‰×#Ñ#¤OÔ4Üˆ�‰ÔÜ�]‰]œ:Ó&ñ 	9Ø�K‰K×'Ñ'¬Ô8÷	9àˆ�‰ÔØ�‰×#Ñ#¤OÔ4Ø�‰×ÑÜ�Tœ9 VÓ,¬i¸Ó.?Ó@ô	Bàˆ�‰ÔÜ�]‰]œ:Ó&ñ 	9Ø�K‰K×'Ñ'¬Ô8÷	9ð 	9÷	9ñ 	9ú÷
	9ð 	9ú÷	9ð 	9ús$   Á F.Ã F;Æ GÆ.F8Æ;GÇGN)rn   ro   rp   rq   r  r  r   r   r!  r)  r-  r0  r2  r5  r8  r;  r?  rB  rG  r5   r%   r#   r  r  3  sâ   „ Ø‡[�[×ÑÐHØ	�o tÐ,Ø	˜+ uÐ-ðKó ñ8ó	ð8ð ‡[�[×ÑÐ2Ø	�dÐ˜o¨uÐ5ð57ó 8ñOó8ðOòIòIò&ð ‡[�[×ÑÐ2Ø	�dÐ˜o¨uÐ5ð57ó 8ñ,ó8ð,ò&ò	IòIò;ó9r%   r  c                   ó   — e Zd Zd„ Zd„ Zd„ Zy)ÚTestSystemExamplesc                 óV  — t        d«      \  }}}}}t        d«      \  }}}}	}
t        d«      }t        d|¬«      }t        d|¬«      }t	        d«      }t        j                  |«      }|j                  |fk(  sJ ‚|j                  |j                  k(  sJ ‚|j                  |j                  k(  sJ ‚t        d||||
|j                  ¬	«      }t        d
||||	|j                  |||j                  z  ¬«      }|j!                  ||«       |j"                  ||fk(  sJ ‚|j%                  d«      |k(  sJ ‚|j'                  d«      |k(  sJ ‚|j)                  | |j                  z  «       |j+                  |j                  ||j                  z  f«       |j-                  t/        ||z  |j                  ||«      «       |j1                  «        |j3                  «        t5        |j6                  t8        «      sJ ‚t;        |j<                  t?        ||z   ||z  tA        |«      z  g||z  tA        |«      z  ||dz  z  gg«      z
  «      tC        dd«      k(  sJ ‚t;        |jD                  t?        ||z  |	dz  z  tG        |«      z  |z   g| |z  |z  tG        |«      z  ||z  z   gg«      z
  «      tC        dd«      k(  sJ ‚|jI                  tK        |j                  jM                  |j                  «      jO                  |j                  «      «      «       |j6                  �J ‚||c|_(        |_)        |	|
c|_*        |_+        |j1                  «        || tG        |«      z  |
| tA        |«      z  |	z  |
jY                  tZ        «      ||	dz  tG        |«      z  |	jY                  tZ        «      tA        |«      z  z
  z  i}| |z  tG        |«      z  ||z  |z  z   ||z  tG        d|z  «      z  |	dz  z  dz  z   ||z  tG        d|z  «      z  |	dz  z  dz  z
  |tA        |«      z  z
  ||tA        |«      dz  z  |tG        |«      dz  z  z   z  z  }t]        t_        |j3                  «       ja                  |«      |	jY                  tZ        «      «      jc                  «       «      d   }t;        ||z
  «      dk(  sJ ‚t5        |j6                  t8        «      sJ ‚t?        ddgddgg«       }t?        |
|	g«       }t?        |dz   |z  tA        |«      dz  z  |dz  |z  z   ||z  tA        |«      z  |||z   z  tA        |«      z  z
  g|tA        |«      z  dgg«      }t?        | |z  |z  tG        |«      z  ||z  z   |dz  |z  |	dz  z  tG        |«      z  tA        |«      z  z
  ||z  tA        |«      z  z
  g||	dz  z  tG        |«      z  gg«      }|je                  tC        dd«      «      jg                  tC        dd«      je                  |«      «      }|jg                  |«      }t;        |j<                  |z
  «      tC        dd«      k(  sJ ‚t;        |jD                  |z
  «      tC        dd«      k(  sJ ‚t;        |jh                  |z
  «      tC        dd«      k(  sJ ‚t;        |jj                  |z
  «      tC        dd«      k(  sJ ‚y )Núg l mc mp kzF qp qc up ucÚrailÚcartre   ÚbobÚ	bob_frameÚslider©Ú
joint_axisÚpin©rR  Úchild_interframeÚchild_pointrB   r   r   rF   )6r   r   r   r   r   r   r…   r2   r   r   rj   r   rl   r   Úzrß   rH   r3   rü   r  Úapply_uniform_gravityr§   rª   r   rw   r  r€   r6   r   r   Úmass_matrixr	   r   r   Úforcingr   rN   r   Úpos_fromÚdotr'   r(   r*   r+   rg   rh   r^   r   ÚxreplaceÚvaluesÚrow_joinÚcol_joinr  r  )r"   ÚgÚlÚmcÚmpÚkÚFÚqpÚqcÚupÚucrL  rM  rN  rO  r    rP  rS  ÚsubsÚupd_expectedÚupd_solÚMkÚgkÚMdÚgdÚMmr  s                              r#   Útest_cart_pendulum_kanesz+TestSystemExamples.test_cart_pendulum_kanes’  s§  € ô " -Ó0‰ˆˆ1ˆb�"�aÜ*¨?Ó;Ñˆˆ2ˆr�2�rÜ˜Ó ˆÜ˜ bÔ)ˆÜ�u 2Ô&ˆÜ" ;Ó/ˆ	Ü×&Ñ& tÓ,ˆØ�}‰}  Ò'Ð'Ð'Ø�|‰|˜tŸz™zÒ)Ð)Ð)Ø×!Ñ! T§_¡_Ò4Ð4Ð4Ü ¨$°°b¸"ÈÏÉÔPˆÜ�u˜d C¨¨R¸D¿F¹FØ(1¸qÀ9Ç;Á;¹ôPˆà×Ñ˜& #Ô&Ø�}‰} ¨ Ò-Ð-Ð-Ø×Ñ Ó)¨VÒ3Ð3Ð3Ø�‰˜uÓ%¨Ò,Ð,Ð,Ø×$Ñ$ a R¨&¯(©(¡]Ô3Ø×Ñ˜$Ÿ/™/¨1¨t¯v©v©:Ð6Ô7Ø×Ñœ^¨A°©F°D·F±F¸IÀtÓLÔMØ×ÑÔ Ø×ÑÔÜ˜&×+Ñ+¬[Ô9Ð9Ð9Ü˜×+Ñ+¬oØ�2‰g�r˜A‘v¤ B£Ñ'Ð(¨2°©6´C¸³GÑ+;¸RÀ!ÀqÁ&¹[Ð*IÐJó/Lñ Ló Mä˜˜A“;òð 	 ð ô ˜Ÿ™¬/Ø�!‰V�b˜A‘gÑ¤ B£Ñ'¨!Ñ+Ð,ØˆS�1‰W�q‰[œ3˜r›7Ñ" Q¨¡VÑ+Ð,ð;.ó +/ñ /ó 0ä38¸¸A³;ò?ð 	@ð ?ð 	×(Ñ(Ü�C—N‘N×+Ñ+¨D¯O©OÓ<×@Ñ@ÀÇÁÓJÓKô	Mà× Ñ Ð(Ð(Ð(Ø%'¨Ð"ˆŒ�f”lØ%'¨Ð"ˆŒ�f”lØ×ÑÔ ð �Q�Bœ˜R›‘LØ�Q�Bœ˜R›‘L 2Ñ%Ø—‘œ“
˜A  q¡¬3¨r«7Ñ!2°R·W±W¼Q³ZÄ#ÀbÃ'Ñ5IÑ!IÑJðLˆð ˆR�"‰W”s˜2“wÑ  R¡¨!¡Ñ+¨a°"©f´s¸1¸r¹6³{Ñ.BÀRÈ1ÁWÑ.LÈqÑ.PÑPØ�2‰vœ˜A ™F›Ñ# b¨A¡gÑ-°Ñ1ñ2Ø45¼¸B»±Kñ@à�"”s˜2“w !‘|Ñ# b¬3¨r«7°a©<Ñ&7Ñ7Ñ8ñ:ð 	ô œ˜f×.Ñ.Ó0×9Ñ9¸$Ó?Ø Ÿg™g¤a›jó*ß*0©&«(ó4Ø45ñ7ˆä˜ ,Ñ.Ó/°1Ò4Ð4Ð4Ü˜&×+Ñ+¬[Ô9Ð9Ð9ô   1˜v¨¨1 vÐ.Ó/Ð/ˆÜ˜r 2˜hÓ'Ð'ˆÜ  Q¡˜w¨™|¬c°"«g¸©lÑ:¸QÀ!¹VÀb¹[ÑHØ  2™v¬¨B«Ñ/°!°r¸B±w±-Ä#ÀbÃ'Ñ2IÑIðKà ¤3 r£7™{¨AÐ.ð0ó 1ˆô Øˆb�1‰f�r‰kœC ›GÑ# a¨"¡fÑ,¨q°A©v¸©{¸RÀ1¹WÑ/DÄsÈ2ÃwÑ/NÜ�"‹gñ0ñ Ø˜A™¤ B£™ñ(ð )Ø+,¨r°Q©w©;¼¸R»Ñ+@Ð*AðCóDˆð �k‰kœ%  1›+Ó&×0Ñ0´°q¸!³×1EÑ1EÀbÓ1IÓJˆØ�[‰[˜‹_ˆÜ˜×*Ñ*¨RÑ/Ó0´E¸!¸Q³KÒ?Ð?Ð?Ü˜Ÿ™¨Ñ+Ó,´°a¸³Ò;Ð;Ð;Ü˜×/Ñ/°"Ñ4Ó5¼¸qÀ!»ÒDÐDÐDÜ˜×+Ñ+¨bÑ0Ó1´U¸1¸a³[Ò@Ð@Ñ@r%   c                 óØ  — t        d«      \  }}}}}t        d«      \  }}}t        dd«      \  }	}
t        d«      }t        d|¬«      }t        d|¬«      }t	        d	«      }t        j                  |«      }|j                  |fk(  sJ ‚|j                  |j                  k(  sJ ‚|j                  |j                  k(  sJ ‚t        d
||||
|j                  ¬«      }t        d||||	|j                  |||j                  z  ¬«      }|j!                  ||«       |j"                  ||fk(  sJ ‚|j%                  d
«      |k(  sJ ‚|j'                  d«      |k(  sJ ‚|j                  D ]U  }|j(                  |z  |j                  j+                  |j                  «      j-                  |j                  «      z  |_        ŒW |j1                  |j                  ||j                  z  f«       |j3                  t5        ||z  |j                  ||«      «       |j7                  t8        «       |j;                  t8        «       t=        |j>                  tA        ||z   ||z  tC        |«      z  g||z  tC        |«      z  ||dz  z  gg«      z
  «      tE        dd«      k(  sJ ‚t=        |jF                  tA        ||z  |	dz  z  tI        |«      z  |z   g| |z  |z  tI        |«      z  ||z  z   gg«      z
  «      tE        dd«      k(  sJ ‚|jK                  tM        |j                  j+                  |j                  «      j-                  |j                  «      «      «       |jN                  �J ‚||c|_(        |_)        || tI        |«      z  |
| tC        |«      z  |	z  |
jU                  tV        «      ||	dz  tI        |«      z  |	jU                  tV        «      tC        |«      z  z
  z  i}| |z  tI        |«      z  ||z  |z  z   ||z  tI        d|z  «      z  |	dz  z  dz  z   ||z  tI        d|z  «      z  |	dz  z  dz  z
  |tC        |«      z  z
  ||tC        |«      dz  z  |tI        |«      dz  z  z   z  z  }|j;                  t8        «      }|jN                  jX                  d   }|jN                  j[                  «       |   }t]        |d   j_                  ||i«      j_                  |«      |	jU                  tV        «      «      d   }t=        ||z
  «      dk(  sJ ‚ta        |jN                  t8        «      sJ ‚tA        |dz  |z  ||z  tC        |«      z  | tC        |«      z  g||z  tC        |«      z  ||z   dgg«      }tA        | |z  |z  tI        |«      z  ||z  z   g||z  tI        |«      z  |	dz  z  |z   gg«      }tc        d«      je                  tE        dd«      «      jg                  tE        dd«      je                  |jg                  tA        |tC        |«      z  ddg«      jh                  «      «      «      }tA        |	|
g|d d  z   |tI        |«      z  |	dz  z  gz   «      }t=        |j>                  |z
  «      tE        dd«      k(  sJ ‚t=        |jF                  |z
  «      tE        dd«      k(  sJ ‚t=        |jj                  |z
  «      tE        dd«      k(  sJ ‚t=        |jl                  |z
  «      tE        dd«      k(  sJ ‚y )NrK  zF qp qczqp qcr   rL  rM  re   rN  rO  rP  rQ  rS  rT  rB   r   r
  rD   r–   )7r   r   r   r   r   r   r…   r2   r   r   rj   r   rl   r   rW  rß   rH   r3   rü   r  rf   r[  r\  Úpotential_energyr§   rª   r   rw   r   r  r   rY  r	   r   r   rZ  r   rN   r   r6   r'   r(   rg   rh   Úlam_vecÚsolve_multipliersr   r]  r€   r   r_  r`  r«   r  r  )r"   ra  rb  rc  rd  re  rf  rg  rh  ÚqpdÚqcdrL  rM  rN  rO  r    rP  rS  rƒ   rk  Úqpdd_expectedÚeomsÚlam1Úlam1_solÚqpdd_solrp  rq  rr  r  s                                r#   Útest_cart_pendulum_lagrangez.TestSystemExamples.test_cart_pendulum_lagrange×  sË  € ô " -Ó0‰ˆˆ1ˆb�"�aÜ" 9Ó-‰	ˆˆ2ˆrÜ! '¨1Ó-‰ˆˆSÜ˜Ó ˆÜ˜ bÔ)ˆÜ�u 2Ô&ˆÜ" ;Ó/ˆ	Ü×&Ñ& tÓ,ˆØ�}‰}  Ò'Ð'Ð'Ø�|‰|˜tŸz™zÒ)Ð)Ð)Ø×!Ñ! T§_¡_Ò4Ð4Ð4Ü ¨$°°b¸#Ø+/¯6©6ô3ˆä�u˜d C¨¨S¸T¿V¹VØ(1¸qÀ9Ç;Á;¹ôPˆà×Ñ˜& #Ô&Ø�}‰} ¨ Ò-Ð-Ð-Ø×Ñ Ó)¨VÒ3Ð3Ð3Ø�‰˜uÓ%¨Ò,Ð,Ð,Ø—M‘Mò 	2ˆDØ$(§I¡I°¡M°D·O±O×4LÑ4LØ×"Ñ"ó5$ß$'¡C¨¯©£Mñ%2ˆDÕ!ð	2ð 	×Ñ˜$Ÿ/™/¨1¨t¯v©v©:Ð6Ô7Ø×Ñœ^¨A°©F°D·F±F¸IÀtÓLÔMØ×ÑœÔ/Ø×ÑœÔ)Ü˜×+Ñ+¬oØ�2‰g�r˜A‘v¤ B£Ñ'Ð(¨2°©6´C¸³GÑ+;¸RÀ!ÀqÁ&¹[Ð*IÐJó/Lñ Ló Mä˜˜A“;òð 	 ð ô ˜Ÿ™¬/Ø�!‰V�c˜Q‘hÑ¤ R£Ñ(¨1Ñ,Ð-°°°a±¸!±¼cÀ"»gÑ0EÈÈBÉÑ0NÐ/Oð;Qó +
ñ 
ó ä�A�q‹kòð 	ð ð 	×(Ñ(Ü�C—N‘N×+Ñ+¨D¯O©OÓ<×@Ñ@ÀÇÁÓJÓKô	Mà× Ñ Ð(Ð(Ð(Ø%'¨Ð"ˆŒ�f”lð �Q�Bœ˜R›‘LØ�a�Rœ#˜b›'‘\ CÑ'Ø—‘œ“˜Q #¨¡(¬S°«WÑ"4°s·x±xÄ³{ÄSÈÃWÑ7LÑ"LÑMðOˆð ˆR�"‰W”s˜2“wÑ  R¡¨!¡Ñ+¨a°"©f´s¸1¸r¹6³{Ñ.BÀSÈAÁXÑ.MØñ/ñ Ø�R‘œ#˜a "™f›+Ñ%¨¨q©Ñ0°1Ñ4ñ5Ø78¼3¸r»7±{ñCà�"”s˜2“w !‘|Ñ# b¬3¨r«7°a©<Ñ&7Ñ7Ñ8ñ:ð 	ð ×Ñ¤Ó0ˆØ× Ñ ×(Ñ(¨Ñ+ˆØ×$Ñ$×6Ñ6Ó8¸Ñ>ˆÜ˜˜a™×)Ñ)¨4°Ð*:Ó;×DÑDÀTÓJØŸ™¤!›ó&Ø&'ñ)ˆä˜ =Ñ0Ó1°QÒ6Ð6Ð6Ü˜&×+Ñ+¬_Ô=Ð=Ð=ô ˜q A™v¨™{¨A°©F´S¸³WÑ,<¸q¸bÄ3ÀrÃ7¹lÐKØ  2™v¬¨B«Ñ/°°b±¸"Ð=ð?ó @ˆäØˆb�1‰f�r‰kœC ›GÑ# a¨"¡fÑ,Ð-Ø�"‰f”s˜2“wÑ ¨¡Ñ)¨AÑ-Ð.ð0ó1ˆô �!‹f�o‰oœe A q›kÓ*×4Ñ4´U¸1¸a³[×5IÑ5IØ�K‰Kœ¨¬S°«W©°a¸Ð(;Ó<×>Ñ>Ó?ó6Aó Bˆä˜c 3˜Z¨"©Q¨%Ñ/°1´s¸2³w±;ÀÈÁÑ3IÐ2JÑJÓKˆÜ˜×*Ñ*¨RÑ/Ó0´E¸!¸Q³KÒ?Ð?Ð?Ü˜Ÿ™¨Ñ+Ó,´°a¸³Ò;Ð;Ð;Ü˜×/Ñ/°"Ñ4Ó5¼¸qÀ!»ÒDÐDÐDÜ˜×+Ñ+¨bÑ0Ó1´U¸1¸a³[Ò@Ð@Ñ@r%   c           	      ó   — t        d«      \  }}}t        d«      \  }}}t        dd¬«      \  }}t        d|¬«      }	t        «       }
|
j	                  |	«       |	j
                  j                  |
j                  ||
j                  z  «       |	j
                  j                  |
j                  ||
j                  z  ||
j                  z  z   «       |g|g|gc|
_        |
_        |
_        |j                  t         «      |z
  g|
_        |
j%                  | |
j                  z  «       |
j'                  t)        |	||
j                  z  «      t)        |	||
j                  z  ||z  |
j                  z  z
  «      «       |
j+                  «        |
j-                  «        t/        dg«      }t/        |g«      }t/        |g«      }t/        |||z  z
  g«      }|j1                  t3        dd«      «      j5                  t3        dd«      j1                  |«      «      }|j5                  |«      }t/        |||z  z
  g«      }t7        |
j8                  |z
  «      t3        dd«      k(  sJ ‚t7        |
j:                  |z
  «      t3        dd«      k(  sJ ‚t7        |
j<                  |z
  «      t3        d	d	«      k(  sJ ‚t7        |
j>                  |z
  «      t3        d	d«      k(  sJ ‚t7        |
j@                  jB                  |z
  «      t3        dd«      k(  sJ ‚y )
Nzg m muzq u uazN FT)Úpositiver¨   re   r   rB   )"r   r   r   r   ri   rj   rk   r   rl   Úset_velr   rß   r'   r*   r-   rg   rh   r.   rX  r§   r
   rw   r  r	   r_  r   r`  r   rY  rZ  r  r  r6   Úauxiliary_eqs)r"   ra  rd   Úmur)   r,   rM   r©   rf  r¨   r    rn  ro  rp  rq  rr  r  Úaux_eqss                     r#   Útest_box_on_groundz%TestSystemExamples.test_box_on_ground  s—  € ô ˜8Ó$‰ˆˆ1ˆbÜ! (Ó+‰ˆˆ1ˆbÜ˜e¨dÔ3‰ˆˆ1Ü�S˜qÔ!ˆÜ“ˆØ×Ñ˜!ÔØ	�‰×Ñ˜V×/Ñ/°°V·X±X±Ô>Ø	�‰×Ñ˜VŸ\™\¨1¨v¯x©x©<¸"¸v¿x¹x¹-Ñ+GÔHØ45°3¸¸¸b¸TÐ0ˆŒ�f”l F¤LØ—v‘vœa“y 1‘}�oˆŒØ×$Ñ$ a R¨&¯(©(¡]Ô3Ø×ÑÜ�!�Q˜Ÿ™‘\Ó"Ü�!�Q˜Ÿ™‘\ B¨¡F¨V¯X©XÑ$5Ñ5Ó6ô	8ð 	×ÑÔ Ø×ÑÔô ˜a˜SÓ!ˆÜ˜a˜SÓ!ˆÜ˜a˜SÓ!ˆÜ˜a " q¡&™j˜\Ó*ˆØ�k‰kœ%  1›+Ó&×0Ñ0´°q¸!³×1EÑ1EÀbÓ1IÓJˆØ�[‰[˜‹_ˆÜ! 1 q¨1¡u¡9 +Ó.ˆÜ˜×*Ñ*¨RÑ/Ó0´E¸!¸Q³KÒ?Ð?Ð?Ü˜Ÿ™¨Ñ+Ó,´°a¸³Ò;Ð;Ð;Ü˜×/Ñ/°"Ñ4Ó5¼¸qÀ!»ÒDÐDÐDÜ˜×+Ñ+¨bÑ0Ó1´U¸1¸a³[Ò@Ð@Ð@Ü˜×)Ñ)×7Ñ7¸'ÑAó Ü" 1 a›[ò)ð 	)ñ )r%   N)rn   ro   rp   rs  r  r†  r5   r%   r#   rI  rI  ‘  s   „ òCAòJEAóN!)r%   rI  ))rq   Úsympy.core.symbolr   Úsympy.core.sympifyr   Ú(sympy.functions.elementary.trigonometricr   r   Úsympy.matrices.denser   r   Úsympy.matrices.immutabler	   Úsympy.physics.mechanicsr
   r   r   r   r   r   r   r   r   r   r   r   r   Úsympy.simplify.simplifyr   Úsympy.solvers.solversr   Ú_trh   r)   rG   r,   rM   r   rt   r  rI  r5   r%   r#   ú<module>r�     s£   ðÛ å %Ý &ß =ß +Ý 4÷÷ ÷ õ õ -Ý 'à×Ñ€Ù�5Ó€Ù�E˜1Ó€Ù�5Ó€Ù�FÓ€÷RLñ RLôjEC�ô ECôP[9˜ô [9÷|n)ò n)r%   