o
    hK                     @   s  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 dZ%G dd de%Z&G dd de%Z'G dd dZ(dS )    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                   @   sj   e Zd Ze dd ZdddZdddZe d	d
 Ze dd Z	dddZ
e dd ZdS )TestSystemBasec                 C   s   t tdtd| _d S )Nframefixed_point)r   r   r   systemself r    c/var/www/vscode/kcb/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_system_class.py_empty_system_setup   s   z"TestSystemBase._empty_system_setupr    c                 C   s|   d}d}|D ]}||vrt | j|d d  g ksJ q|D ]}||vr-t | j|dks-J qd|vr:| jjd u s<J d S d S )N)
q_indq_depqu_indu_depuu_auxkdesholonomic_constraintsnonholonomic_constraints)loadsbodiesjoints	actuatorsr    
eom_method)getattrr   r1   )r   excludematricestuplesattrr    r    r!   _empty_system_check   s   z"TestSystemBase._empty_system_checkTc              	   C   s   t tdtd| _|rtdnt}tdtd| _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d | jj|d dd |r| j|d td   | jtd td  | jt
d
 t
d  t
d   | j|d td  |d
   |d d
 | j_|d
d | j_| jjd d  | jjd d  | _| _| jjd d  | jjd d  | _| _| jjd d  | _| jjd d  | _| jjd d  | _| jjd d  | _d S )Nr   r   r   zrb1:5clsJ1r   r   J2   J3   F)independent   ) r   r   r   r   r   qdr   r   r.   r   r%   r   r/   
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vcr,   nhc)r   with_speedsr(   r    r    r!   _create_filled_system(   s.   """  &&z$TestSystemBase._create_filled_systemc                 C      | j dd d S )NTrN   rO   r   r    r    r!   _filled_system_setupB      z#TestSystemBase._filled_system_setupc                 C   rP   )NFrQ   rR   r   r    r    r!   _filled_system_setup_no_speedsF   rT   z-TestSystemBase._filled_system_setup_no_speedsc                 C   s^  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 sO| j jd d  td d ksOJ d|v sc| j jd d  tdd kscJ d	|v sw| j jd d  td d kswJ d
|v s| j jd d  td d ksJ d|v s| j j	d d  dd t
td d td d D ksJ d|v s| j jd d  td td  td  gksJ d|v s| j jd d  td td  td  gksJ d|v s	| j jd d  td td  td  td td  td  gks	J d|v s| j jt| jksJ d|v s+| j jt| jks-J d S d S )Nr#   r>   r$   r%   r@   r&   r<   r'   r(   r)   r*   c                 S      g | ]\}}|| qS r    r    .0uiqdir    r    r!   
<listcomp>R   s    z7TestSystemBase._filled_system_check.<locals>.<listcomp>r+   r   r   r,   rK   r.   r/   )r   r#   r%   r$   r&   r(   r'   r)   rG   r*   ziprA   r+   r,   rK   r.   tupler/   )r   r3   r    r    r!   _filled_system_checkJ   s0   (&(((((..
,

z#TestSystemBase._filled_system_checkc                 C   sp   t d | j_td | j_td t d t | j_tdt	dd}| j
| |j| jjt d | jj  d S )Nr   pmmass)r%   r   r#   r(   r&   difftr*   r   r   
add_bodies
masscenterset_posr   x)r   r"   r_   r    r    r!   _moving_point_massa   s   "z!TestSystemBase._moving_point_massN)r    )T)__name__
__module____qualname__pytestfixturer"   r7   rO   rS   rU   r^   ri   r    r    r    r!   r      s    





r   c                   @   s
  e Zd Zdd Zdd Zejddedgejdde	dgdd	 Z
d
d Zdd Ze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d Zejdddge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d  Ze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"d# Zejd$edd i edd fgd%d& Ze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'd( Ze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d0d1ed ed  fi fd2d3ed ed  fi fd4d5ed ed  fi fd6d7ed8fi fd9d:ee	d;ed<jfi fd=d>eed?ed<jed@fi fgdAdB ZejdCd*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d0d1dDefd2d3dDefd4d5dDefd6d7edefd9d:edefd=d>edefgdEdF ZejdGdHi dIdJ eeddK eddK D fedK edK  ed ed  fi dLdJ eeddM eddM D fgdNdO Z ejded ed  edK edK  fi fed ed   edK edK  fi fed ed  edK edK  gi fgdPdQ Z!ejdRdHi ed ed  ed  gfedK ed  ed ed  fi ed ed  ed  edK ed  ed ed  gfgdSdT Z"ejded ed  ed  edK ed  fi fed ed  ed   edK ed  fi fed ed  edK ed  fi fgdUdV Z#ejdRdHi ed ed  ed  gfedK ed  ed ed  fi ed ed  ed  edK ed  ed ed  gfgdWdX Z$ejded ed  ed  edK ed  fi fed ed  ed   edK ed  fi fed ed  edK ed  fi fed ed  edK ed  gi fgdYdZ Z%ejd[g g fed ed  ed  ed ed  ed  gfed ed  ed ed  ged ed  ed ed  gfgd\d] Z&d^d_ Z'd`da Z(dbdc Z)ddde Z*dfdg Z+dhdi Z,ejdjg dkdldm Z-ejdng dodpdq Z.ejdre/e0gdsdt Z1ejdui e2dvdgdedwggfdxdie2ddgdedwggfgdydz Z3ejd{i e2ddgdedwgge2ed 4e5dgfgd|d} Z6ejd~e/ddiefe0ddiefe/d6g iefe/dg iefe0d6g iefe0ddiefgdd Z7dS )
TestSystemc                 C      |    | j  d S N)r7   r   validate_system)r   r"   r    r    r!   test_empty_systeml      zTestSystem.test_empty_systemc                 C   rp   rq   )r^   r   rr   r   rS   r    r    r!   test_filled_systemp   rt   zTestSystem.test_filled_systemr   Nr   c                 C   s,  |d u r|d u rt  | _nt ||| _|d u r!| jjjdks J n| jj|ks)J |d u r7| jjjdks6J n| jj|ks?J |   t| jjtsLJ t| jj	tsUJ t| jj
ts^J t| jjtsgJ t| jjtspJ t| jjtsyJ t| jjtsJ t| jjtsJ t| jjtsJ d S )Ninertial_pointinertial_frame)r   r   r   namer   r7   
isinstancer#   r   r$   r%   r&   r'   r(   r*   r+   r,   )r   r   r   r    r    r!   	test_initt   s&   
zTestSystem.test_initc                 C   sR   t d}t|| _| jj|jksJ | jj|jksJ | jdd |f| j_d S )Nbody)r.   r3   )	r   r   from_newtonianr   r   rf   r   r7   r.   )r   rbr    r    r!   test_from_newtonian_rigid_body   s   z)TestSystem.test_from_newtonian_rigid_bodyc                 C   sB   t d}tt t| W d    d S 1 sw   Y  d S )Nparticle)r   rm   raises	TypeErrorr   r~   )r   ptr    r    r!   test_from_newtonian_particle   s   "z'TestSystem.test_from_newtonian_particlez)args, kwargs, exp_q_ind, exp_q_dep, exp_qr>   r?   TF)TFTr   r<   r   c                 C      | 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d || j _|| j _| j jd d  |ksGJ | j jd d  |ksSJ | j jd d  |ks_J | jdd d S )N)r#   r$   r%   r}   )r   rC   r#   r$   r%   r7   )r   r"   argskwargs	exp_q_ind	exp_q_depexp_qr    r    r!   test_coordinates      
zTestSystem.test_coordinatesfuncrC   rD   zargs, kwargs   ac                 C   sL   t t t| j||i | W d    n1 sw   Y  |   d S rq   )rm   r   
ValueErrorr2   r   r^   )r   rS   r   r   r   r    r    r!   test_coordinates_speeds_invalid   s   z*TestSystem.test_coordinates_speeds_invalidz)args, kwargs, exp_u_ind, exp_u_dep, exp_uc                 C   r   )N)r&   r'   r(   r}   )r   rD   r&   r'   r(   r7   )r   r"   r   r   	exp_u_ind	exp_u_depexp_ur    r    r!   test_speeds   r   zTestSystem.test_speedszargs, kwargs, exp_u_auxc                 C   sf   | j j|i | | j jd d  |ksJ | jdd || j _| j jd d  |ks+J | jdd d S )N)r)   r}   )r   rF   r)   r7   )r   r"   r   r   	exp_u_auxr    r    r!   test_auxiliary_speeds   s   z TestSystem.test_auxiliary_speedsc                 C   H   t t | jj|i | W d    n1 sw   Y  |   d S rq   )rm   r   r   r   rF   r^   r   rS   r   r   r    r    r!   test_auxiliary_invalid      z!TestSystem.test_auxiliary_invalidzprop, add_func, args, kwargsr#   r$   r&   r'   r)   rF   r*   rE   r+   rH   r,   rI   r.   re   r|   r-   	add_loadsPNr0   add_actuatorsTAc                 C   s   t | j|d |ddf}|dv r|d7 }| j|d tt| j|d d  g ks*J t| j||i | tt| j|d d  t|ksGJ d S )Nr    r%   r(   )r+   r,   rK   r}   )setattrr   r^   listr2   )r   rS   propadd_funcr   r   r3   r    r    r!   test_add_after_reset   s   
 (zTestSystem.test_add_after_resetzprop, add_func, value, error   c                 C   s~   t | t| j|| W d    n1 sw   Y  t | t| j|| W d    n1 s4w   Y  |   d S rq   )rm   r   r2   r   r   r^   )r   rS   r   r   valueerrorr    r    r!   test_type_error  s   zTestSystem.test_type_errorzargs, kwargs, exp_kdesr    c                 C   rV   r    r    rW   r    r    r!   r[   #      zTestSystem.<listcomp>r@   c                 C   rV   r    r    rW   r    r    r!   r[   %  r      c                 C   sf   | j j|i | | jdd | j jd d  |ksJ || j _| jdd | j jd d  |ks1J d S )N)r*   r}   )r   rE   r^   r*   )r   rS   r   r   exp_kdesr    r    r!   	test_kdes"  s   zTestSystem.test_kdesc                 C   r   rq   )rm   r   r   r   rE   r^   r   r    r    r!   test_kdes_invalid1  s   zTestSystem.test_kdes_invalidzargs, kwargs, exp_conc                 C   s   d}dd |D | j  }| jj|i | | j|d | jjd d  |ks'J | jjd d  |ks3J || j_| j|d | jjd d  |ksIJ | jjd d  |ksUJ d S )N)r+   rK   c                 S   s   g | ]}| tqS r    )rc   rd   )rX   cr    r    r!   r[   C  s    z9TestSystem.test_holonomic_constraints.<locals>.<listcomp>r}   )rM   r   rH   r^   r+   rK   r   rS   r   r   exp_conr3   exp_vel_conr    r    r!   test_holonomic_constraints;  s   z%TestSystem.test_holonomic_constraintsc                 C   r   rq   )rm   r   r   r   rH   r^   r   r    r    r!   "test_holonomic_constraints_invalidO  r   z-TestSystem.test_holonomic_constraints_invalidc                 C   s   d}| j d t| j | }| jj|i | | j|d | jjd d  |ks)J | jjd d  |ks5J || j_| j|d | jjd d  |ksKJ | jjd d  |ksWJ d S )N)r,   rK   r}   )rL   lenrJ   r   rI   r^   r,   rK   r   r    r    r!   test_nonholonomic_constraintsZ  s   z(TestSystem.test_nonholonomic_constraintsc                 C   r   rq   )rm   r   r   r   rI   r^   r   r    r    r!   %test_nonholonomic_constraints_invalidn  s   z0TestSystem.test_nonholonomic_constraints_invalidzconstraints, expectedc                 C   s0   || j _| jdd | j jd d  |ksJ d S )Nr   r}   )r   rK   r^   )r   rS   constraintsexpectedr    r    r!   #test_velocity_constraints_overwritez  s   z.TestSystem.test_velocity_constraints_overwritec                 C   sZ   t d t d  | j_| jdd | jjd d  t d t d  gks#J d | j_|   d S )Nr>   r<   r   r}   )rA   r   rK   r^   ru   r    r    r!   &test_velocity_constraints_back_to_auto  s
   &z1TestSystem.test_velocity_constraints_back_to_autoc                    s:  t dt d}}tdtd} j|| jjg j||R ks)J j  jjg j|| R ksAJ g j_jjdksMJ  j_jj fksZJ tdttfdd tt	 fd	d tt ||| fj_W d    n1 sw   Y  jj fksJ d S )
Nrb1rb2p1p2r    symbc                      s    j S rq   r   re   r    )r   r   r    r!   <lambda>      z(TestSystem.test_bodies.<locals>.<lambda>c                         j  S rq   r   r    )r   r   r    r!   r     r   )
r   r   r   re   r.   r   rm   r   r   r   )r   rS   r   r   r   r    )r   r   r   r!   test_bodies  s"    $zTestSystem.test_bodiesc                    s
  t  tdtd }td d}td}td|}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sRJ t
t fdd t
t   jf_	W d    n1 ssw   Y  j	||jffksJ d S )	Nr   r   r   )r   mc1r   c                      s      j jfS rq   )r   rh   yr    r   r   r    r!   r     s    z+TestSystem.test_add_loads.<locals>.<lambda>)r   r   r   r   r   r   r   rh   r	   r-   rm   r   r   r   )r   r   r   r   r   r    r   r!   test_add_loads  s   
$&zTestSystem.test_add_loadsc                 C   s   t  }tdtd}}ttd|j|}ttd|j||}|| |j|fks,J |jdks3J |f|_|j|fks?J d S )Nr   r   T1T2r    )	r   r   r   r   rh   r   r   r0   r-   )r   r   r   r   act1act2r    r    r!   test_add_actuators  s   
zTestSystem.test_add_actuatorsc                    sd  t d\}}}}}}}tdtd\}}	}
}td|||}td||	|| td|	|
||}td|
|||t}t | j	|fksIJ j
|fksRJ jt|gks\J jt|gksfJ jt||t gksuJ |
 | ||t  | j	||fksJ j
||
|	fksJ jt||gksJ jt||gksJ jt||t ||t gksJ ||t     j	|| fksJ j
||
|	fksJ jt|||gksJ jt|||gksJ jt||t ||t ||t  gks(J | j	|| |fks9J j
||
|	|fksFJ jt||||gksTJ jt||||tgkseJ jt||t ||t ||t  gksJ jd d  g ksJ jd d  g ksJ tt fdd	 ttfd
d	 d S )Nz	q1:5 u1:4zrb1:6r8   r:   r;   r=   J_lagc                      
     S rq   rB   r    )r;   r   r    r!   r        
 z,TestSystem.test_add_joints.<locals>.<lambda>c                      r   rq   r   r    )r   r   r    r!   r     r   )r   r   r   r   r   rc   rd   r   rB   r/   r.   r#   r   r&   r*   re   rC   rE   r$   r'   rm   r   r   r   )r   q1q2q3q4u1u2u3r   rb3rb4rb5r:   r=   r   r    )r;   r   r   r!   test_add_joints  sZ   






"zTestSystem.test_add_jointsc                 C   s^   | j dd  | j_ | jj | j dd  ksJ | jdd d| j_d| j_| j | j_ |   d S )Nr   )r/   r}   r    )r/   r   r^   r#   r&   ru   r    r    r!   test_joints_setter  s   
zTestSystem.test_joints_setterzname, joint_index))r:   r   )r;   r   not_existingNc                 C   :   | j |}|d u r|d u sJ d S || j| ksJ d S rq   )r   	get_jointr/   )r   rS   ry   joint_indexjointr    r    r!   test_get_joint     zTestSystem.test_get_jointzname, body_index))r   r   )r   r<   r   c                 C   r   rq   )r   get_bodyr.   )r   rS   ry   
body_indexr|   r    r    r!   test_get_body  r   zTestSystem.test_get_bodyr1   c                 C   s4   G dd d|}| j j|d t| j j|sJ d S )Nc                   @   s   e Zd ZdS )z:TestSystem.test_form_eoms_calls_subclass.<locals>.MyMethodN)rj   rk   rl   r    r    r    r!   MyMethod  s    r   )r1   )r   	form_eomsrz   r1   )r   ri   r1   r   r    r    r!   test_form_eoms_calls_subclass  s   z(TestSystem.test_form_eoms_calls_subclasszkwargs, expectedr`   explicit_kinematicsc                 C   s&   | j jdi | | j j|ksJ d S )Nr    )r   r   mass_matrix_full)r   ri   r   r   r    r    r!   !test_system_kane_form_eoms_kwargs  s   z,TestSystem.test_system_kane_form_eoms_kwargszkwargs, mm, gmc                 C   s:   | j jddti| | j j|ksJ | j j|ksJ d S )Nr1   r    )r   r   r   r   forcing_full)r   ri   r   mmgmr    r    r!   %test_system_lagrange_form_eoms_kwargs  s   z0TestSystem.test_system_lagrange_form_eoms_kwargszeom_method, kwargs, errornon_existing_kwargkd_eqs
Lagrangianc                 C   s   t d | j_tdtdd}| j| |j| jjt d | jj	  t
| | jjdd|i| W d    d S 1 s?w   Y  d S )Nr   r_   r`   ra   r1   r    )r%   r   r#   r   r   re   rf   rg   r   rh   rm   r   r   )r   r"   r1   r   r   r_   r    r    r!   test_form_eoms_kwargs_errors!  s   
"z'TestSystem.test_form_eoms_kwargs_errors)8rj   rk   rl   rs   rv   rm   markparametrizer   r   r{   r   r   r%   r   r(   r   r   r   rG   r   r   rA   r   r	   rh   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   rc   rd   r   r   r    r    r    r!   ro   k   sJ   $((*
$((*






( "

"$"
 4
*,"
 4
*,""
0>
,	





ro   c                   @   s   e Zd Zejdeedfeedfgdd Zejdedfedfgdd Z	d	d
 Z
dd Zdd Zejdedfedfgdd Zdd Zdd Zdd Zdd Zdd ZdS )TestValidateSystemz)valid_method, invalid_method, with_speedsTFc                 C   sT   | j |d | j| tt | j| W d    d S 1 s#w   Y  d S )NrQ   )rO   r   rr   rm   r   r   )r   valid_methodinvalid_methodrN   r    r    r!   test_only_valid4  s
   "z"TestValidateSystem.test_only_validzmethod, with_speedsc                    s\   j |d jdd  j_jd d j_jd d j_tt fdd d S )NrQ   r   r   c                      r   rq   r   rr   r    methodr   r    r!   r   F  r   zBTestValidateSystem.test_missing_joint_coordinate.<locals>.<lambda>)rO   r#   r   r&   r*   rm   r   r   r   r  rN   r    r  r!   test_missing_joint_coordinate?  s
   z0TestValidateSystem.test_missing_joint_coordinatec                    sN    j d d  j_  jdd   j_ jd d  j_tt fdd d S )Nr   r   c                      
    j  S rq   r  r    r   r    r!   r   L  r   z=TestValidateSystem.test_missing_joint_speed.<locals>.<lambda>)r#   r   r&   r*   rm   r   r   ru   r    r   r!   test_missing_joint_speedH  s   z+TestValidateSystem.test_missing_joint_speedc                    s*    j dd   j_ tt fdd d S )Nr   c                      r  rq   r  r    r   r    r!   r   P  r   z<TestValidateSystem.test_missing_joint_kdes.<locals>.<lambda>)r*   r   rm   r   r   ru   r    r   r!   test_missing_joint_kdesN  s   z*TestValidateSystem.test_missing_joint_kdesc                 C   s.   | j d  g| j dd   | j_ | j  d S )Nr   r   )r*   r   rr   ru   r    r    r!   test_negative_joint_kdesR  s    z+TestValidateSystem.test_negative_joint_kdesc                    s|   j |d g j_jjd jd  jd  g j_tt	 fdd g j_
jj
 j_j  d S )NrQ   r   r   c                      r   rq   r  r    r  r    r!   r   ]  r   zFTestValidateSystem.test_missing_holonomic_constraint.<locals>.<lambda>)rO   r   r+   rM   r&   r'   r,   rm   r   r   r$   r#   rr   r  r    r  r!   !test_missing_holonomic_constraintV  s   
z4TestValidateSystem.test_missing_holonomic_constraintc                    sN   g  j _tt fdd  jd  j _ j jd g  j _ j   d S )Nc                      r  rq   r  r    r   r    r!   r   d  r   zITestValidateSystem.test_missing_nonholonomic_constraint.<locals>.<lambda>r   r   )r   r,   rm   r   r   r'   r&   rr   ru   r    r   r!   $test_missing_nonholonomic_constraintb  s
   z7TestValidateSystem.test_missing_nonholonomic_constraintc                    s    j td g  j_  jtd td  g  j_ j   j j_ j d d  j_  jd d  j_tt	 fdd d S )Nr   r   c                      r  rq   r  r    r   r    r!   r   r  r   zFTestValidateSystem.test_number_of_coordinates_speeds.<locals>.<lambda>)
r&   r(   r   r*   rA   rr   r#   rm   r   r   ru   r    r   r!   !test_number_of_coordinates_speedsi  s   

z4TestValidateSystem.test_number_of_coordinates_speedsc                    sb    j d d  j_ tt fdd  j td td  td  g  j_ tt fdd d S )Nr   c                      r  rq   r  r    r   r    r!   r   w  r   z8TestValidateSystem.test_number_of_kdes.<locals>.<lambda>r<   r   c                      r  rq   r  r    r   r    r!   r   y  r   )r*   r   rm   r   r   r(   rA   ru   r    r   r!   test_number_of_kdest  s   $z&TestValidateSystem.test_number_of_kdesc                 C   s   | j jdd d S )NT)check_duplicatesr  ru   r    r    r!   test_duplicates{  s   z"TestValidateSystem.test_duplicatesc                 C   s  t d t| j | j_tt | jt W d    n1 s!w   Y  g | j_| jt t	| j_
tt | jt W d    n1 sJw   Y  g | j_
| jt | jtdtdtd g | j_tt | jt W d    d S 1 sw   Y  d S )NJurbu1rbu2)r(   r   r&   r   rm   r   r   rr   r   rG   r)   rB   r   r   )r   rU   r    r    r!   test_speeds_in_lagrange  s&   "z*TestValidateSystem.test_speeds_in_lagrangeN)rj   rk   rl   rm   r   r   r
   r   r  r  r  r	  r
  r  r  r  r  r  r  r    r    r    r!   r   3  s,    



r   c                   @   s$   e Zd Zdd Zdd Zdd ZdS )TestSystemExamplesc              
   C   s$  t d\}}}}}td\}}}}	}
td}td|d}td|d}td}t|}|j|fks3J |j|jks;J |j	|j
ksCJ td||||
|jd	}td
||||	|j|||j d}||| |j||fksmJ |d|ksvJ |d|ksJ || |j  ||j
||j f |t|| |j|| |  |  t|jtsJ t|jt|| || t | g|| t | ||d  gg t!ddksJ t|j"t|| |	d  t#| | g| | | t#| ||  gg t!ddksJ |$t%|j
&|j
'|j |jd u sJ |||_(|_)|	|
|_*|_+|  || t#| |
| t | |	 |
,t-||	d t#| |	,t-t |   i}| | t#| || |  || t#d|  |	d  d  || t#d|  |	d  d  |t |  ||t |d  |t#|d     }t.t/| 0||	,t-1 d }t|| dksJ t|jtsJ tddgddgg }t|
|	g }t|d  | t |d  |d |  || t | |||  t |  g|t | dgg}t| | | t#| ||  |d | |	d  t#| t |  || t |  g||	d  t#| gg}|2t!dd3t!dd2|}|3|}t|j| t!ddkscJ t|j"| t!ddksrJ t|j4| t!ddksJ t|j5| t!ddksJ d S )Ng l mc mp kzF qp qc up ucrailcartra   bob	bob_frameslider
joint_axispinr  child_interframechild_pointr<   r   r   r@   )6r   r   r   r   r   r   r~   r.   r   r   rf   r   rh   r   zr   rB   r/   r   r   apply_uniform_gravityr   r   r   rr   r   rz   r1   r
   r   mass_matrixr   r   r   forcingr   rH   r   pos_fromdotr#   r$   r&   r'   rc   rd   r]   r   xreplacevaluesrow_joincol_joinr   r   )r   glmcmpkFqpqcupucr  r  r  r  r   r  r  subsupd_expectedupd_solMkgkMdgdMmr   r    r    r!   test_cart_pendulum_kanes  s   

2
,<
"&"6"
"z+TestSystemExamples.test_cart_pendulum_kanesc              
   C   s(  t d\}}}}}td\}}}tdd\}	}
td}td|d}td|d}td	}t|}|j|fks8J |j|jks@J |j	|j
ksHJ td
||||
|jd}td||||	|j|||j d}||| |j||fksrJ |d
|ks{J |d|ksJ |jD ]}|j| |j
|j	|j |_q||j
||j f |t|| |j|| |t |t t|jt || || t!| g|| t!| ||d  gg t"ddksJ t|j#t || |	d  t$| | g| | | t$| ||  gg t"ddksJ |%t&|j
|j
|j |j'd u s+J |||_(|_)|| t$| |
| t!| |	 |
*t+||	d t$| |	*t+t!|   i}| | t$| || |  || t$d|  |	d  d  || t$d|  |	d  d  |t!|  ||t!|d  |t$|d     }|t}|j'j,d }|j'- | }t.|d /||i/||	*t+d }t|| dksJ t0|j'tsJ t |d | || t!| | t!| g|| t!| || dgg}t | | | t$| ||  g|| t$| |	d  | gg}t1d2t"dd3t"dd2|3t |t!| ddgj4}t |	|
g|d d   |t$| |	d  g }t|j| t"ddkseJ t|j#| t"ddkstJ t|j5| t"ddksJ t|j6| t"ddksJ d S )Nr  zF qp qczqp qcr   r  r  ra   r  r  r  r  r  r  r<   r   r   r>   r   )7r   r   r   r   r   r   r~   r.   r   r   rf   r   rh   r   r"  r   rB   r/   r   r   rb   r&  r'  potential_energyr   r   r   rr   r   r   r   r$  r   r   r   r%  r   rH   r   r1   r#   r$   rc   rd   lam_vecsolve_multipliersr   r(  rz   r   r*  r+  r   r   r   )r   r,  r-  r.  r/  r0  r1  r2  r3  qpdqcdr  r  r  r  r   r  r  r|   r6  qpdd_expectedeomslam1lam1_solqpdd_solr;  r<  r=  r   r    r    r!   test_cart_pendulum_lagrange  s   




2:
,6
"
(."z.TestSystemExamples.test_cart_pendulum_lagrangec                 C   s   t d\}}}td\}}}tddd\}}td|d}	t }
|
|	 |	j|
j||
j  |	j	|
j
||
j ||
j   |g|g|g|
_|
_|
_|t| g|
_|
| |
j  |
t|	||
j t|	||
j || |
j   |
  |
  tdg}t|g}t|g}t|||  g}|tddtdd|}||}t|||  g}t|
j| tddksJ t|
j| tddksJ t|
j| td	d	ksJ t|
j| td	dksJ t|
j j!| tddksJ d S )
Nzg m muzq u uazN FT)positiver   ra   r   r<   )"r   r   r   r   re   rf   rg   r   rh   set_velr   r   r#   r&   r)   rc   rd   r*   r#  r   r	   rr   r   r   r*  r   r+  r   r$  r%  r   r   r1   auxiliary_eqs)r   r,  r`   mur%   r(   rG   r   r1  r   r   r9  r:  r;  r<  r=  r   aux_eqsr    r    r!   test_box_on_ground  s>   
 


"
z%TestSystemExamples.test_box_on_groundN)rj   rk   rl   r>  rI  rO  r    r    r    r!   r    s    EGr  ))rm   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   _trd   r%   rA   r(   rG   r   ro   r   r  r    r    r    r!   <module>   s*    <
U   K^