o
    GZhK                     @   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    ^/var/www/auris/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   excludeZmatricesZ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rL   rM   r   r    r    r!   _filled_system_setupB      z#TestSystemBase._filled_system_setupc                 C   rN   )NFrO   rP   r   r    r    r!   _filled_system_setup_no_speedsF   rR   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    .0ZuiZqdir    r    r!   
<listcomp>R   s    z7TestSystemBase._filled_system_check.<locals>.<listcomp>r+   r   r   r,   rI   r.   r/   )r   r#   r%   r$   r&   r(   r'   r)   rE   r*   zipr?   r+   r,   rI   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__pytestZfixturer"   r5   rM   rQ   rS   rZ   re   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)r5   r   validate_system)r   r"   r    r    r!   test_empty_systeml      zTestSystem.test_empty_systemc                 C   rk   rl   )rZ   r   rm   r   rQ   r    r    r!   test_filled_systemp   ro   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 )NZinertial_pointZinertial_frame)r   r   r   namer   r5   
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   rb   r   r5   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 )NZparticle)r   ri   raises	TypeErrorr   rw   )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%   rv   )r   rA   r#   r$   r%   r5   )r   r"   argskwargsZ	exp_q_indZ	exp_q_depZexp_qr    r    r!   test_coordinates      
zTestSystem.test_coordinatesfuncrA   rB   zargs, kwargs   ac                 C   sL   t t t| j||i | W d    n1 sw   Y  |   d S rl   )ri   rz   
ValueErrorr2   r   rZ   )r   rQ   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(   rv   )r   rB   r&   r'   r(   r5   )r   r"   r   r   Z	exp_u_indZ	exp_u_depZ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)   rv   )r   rD   r)   r5   )r   r"   r   r   Z	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 rl   )ri   rz   r   r   rD   rZ   r   rQ   r   r   r    r    r!   test_auxiliary_invalid      z!TestSystem.test_auxiliary_invalidzprop, add_func, args, kwargsr#   r$   r&   r'   r)   rD   r*   rC   r+   rF   r,   rG   r.   ra   ru   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,   rI   rv   )setattrr   rZ   listr2   )r   rQ   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 rl   )ri   rz   r2   r   r   rZ   )r   rQ   r   r   valueerrorr    r    r!   test_type_error  s   zTestSystem.test_type_errorzargs, kwargs, exp_kdesr    c                 C   rT   r    r    rU   r    r    r!   rW   #      zTestSystem.<listcomp>r>   c                 C   rT   r    r    rU   r    r    r!   rW   %  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*   rv   )r   rC   rZ   r*   )r   rQ   r   r   Zexp_kdesr    r    r!   	test_kdes"  s   zTestSystem.test_kdesc                 C   r   rl   )ri   rz   r   r   rC   rZ   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+   rI   c                 S   s   g | ]}| tqS r    )r_   r`   )rV   cr    r    r!   rW   C  s    z9TestSystem.test_holonomic_constraints.<locals>.<listcomp>rv   )rK   r   rF   rZ   r+   rI   r   rQ   r   r   Zexp_conr3   Zexp_vel_conr    r    r!   test_holonomic_constraints;  s   z%TestSystem.test_holonomic_constraintsc                 C   r   rl   )ri   rz   r   r   rF   rZ   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,   rI   rv   )rJ   lenrH   r   rG   rZ   r,   rI   r   r    r    r!   test_nonholonomic_constraintsZ  s   z(TestSystem.test_nonholonomic_constraintsc                 C   r   rl   )ri   rz   r   r   rG   rZ   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   rv   )r   rI   rZ   )r   rQ   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   rv   )r?   r   rI   rZ   rp   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 rl   r   ra   r    )r   r   r    r!   <lambda>      z(TestSystem.test_bodies.<locals>.<lambda>c                         j  S rl   r   r    )r   r   r    r!   r     r   )
r   r   r   ra   r.   r   ri   rz   r{   r   )r   rQ   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 rl   )r   rd   yr    r   r   r    r!   r     s    z+TestSystem.test_add_loads.<locals>.<lambda>)r   r   r   r   r   r   r   rd   r	   r-   ri   rz   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   ZT1ZT2r    )	r   r   r   r   rd   r   r   r0   r-   )r   r   r   r   Zact1Z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:6r6   r8   r9   r;   J_lagc                      
     S rl   r@   r    )r9   r   r    r!   r        
 z,TestSystem.test_add_joints.<locals>.<lambda>c                      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*   ra   rA   rC   r$   r'   ri   rz   r   r{   )r   q1q2Zq3Zq4u1u2u3r   rb3Zrb4Zrb5r8   r;   r   r    )r9   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/   rv   r    )r/   r   rZ   r#   r&   rp   r    r    r!   test_joints_setter  s   
zTestSystem.test_joints_setterzname, joint_index))r8   r   )r9   r   Znot_existingNc                 C   :   | j |}|d u r|d u sJ d S || j| ksJ d S rl   )r   	get_jointr/   )r   rQ   rr   Zjoint_indexZjointr    r    r!   test_get_joint     zTestSystem.test_get_jointzname, body_index))r   r   )r   r:   r   c                 C   r   rl   )r   get_bodyr.   )r   rQ   rr   Z
body_indexru   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)rf   rg   rh   r    r    r    r!   MyMethod  s    r   )r1   )r   	form_eomsrs   r1   )r   re   r1   r   r    r    r!   test_form_eoms_calls_subclass  s   z(TestSystem.test_form_eoms_calls_subclasszkwargs, expectedr\   Zexplicit_kinematicsc                 C   s&   | j jdi | | j j|ksJ d S )Nr    )r   r   mass_matrix_full)r   re   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   re   r   mmgmr    r    r!   %test_system_lagrange_form_eoms_kwargs  s   z0TestSystem.test_system_lagrange_form_eoms_kwargszeom_method, kwargs, errorZnon_existing_kwargZkd_eqsZ
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\   r]   r1   r    )r%   r   r#   r   r   ra   rb   rc   r   rd   ri   rz   r   )r   r"   r1   r   r   r[   r    r    r!   test_form_eoms_kwargs_errors!  s   
"z'TestSystem.test_form_eoms_kwargs_errors)8rf   rg   rh   rn   rq   ri   markparametrizer   r   rt   ry   r}   r%   r   r(   r   r   r   rE   r   r   r?   r   r	   rd   r   r   r   r{   r   rX   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    r    r!   rj   k   sJ   $((*
$((*






( "

"$"
 4
*,"
 4
*,""
0>
,	





rj   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 )NrO   )rM   r   rm   ri   rz   r   )r   Zvalid_methodZinvalid_methodrL   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 )NrO   r   r   c                      r   rl   r   rm   r    methodr   r    r!   r   F  r   zBTestValidateSystem.test_missing_joint_coordinate.<locals>.<lambda>)rM   r#   r   r&   r*   ri   rz   r   r   r   rL   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 rl   r   r    r   r    r!   r   L  r   z=TestValidateSystem.test_missing_joint_speed.<locals>.<lambda>)r#   r   r&   r*   ri   rz   r   rp   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   rl   r   r    r   r    r!   r   P  r   z<TestValidateSystem.test_missing_joint_kdes.<locals>.<lambda>)r*   r   ri   rz   r   rp   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   rm   rp   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 )NrO   r   r   c                      r   rl   r   r    r   r    r!   r   ]  r   zFTestValidateSystem.test_missing_holonomic_constraint.<locals>.<lambda>)rM   r   r+   rK   r&   r'   r,   ri   rz   r   r$   r#   rm   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   rl   r   r    r   r    r!   r   d  r   zITestValidateSystem.test_missing_nonholonomic_constraint.<locals>.<lambda>r   r   )r   r,   ri   rz   r   r'   r&   rm   rp   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   rl   r   r    r   r    r!   r   r  r   zFTestValidateSystem.test_number_of_coordinates_speeds.<locals>.<lambda>)
r&   r(   r   r*   r?   rm   r#   ri   rz   r   rp   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   rl   r   r    r   r    r!   r   w  r   z8TestValidateSystem.test_number_of_kdes.<locals>.<lambda>r:   r   c                      r   rl   r   r    r   r    r!   r   y  r   )r*   r   ri   rz   r   r(   r?   rp   r    r   r!   test_number_of_kdest  s   $z&TestValidateSystem.test_number_of_kdesc                 C   s   | j jdd d S )NT)Zcheck_duplicatesr   rp   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 )NZJuZrbu1Zrbu2)r(   r   r&   r   ri   rz   r   rm   r   rE   r)   r@   r   r   )r   rS   r    r    r!   test_speeds_in_lagrange  s&   "z*TestValidateSystem.test_speeds_in_lagrangeN)rf   rg   rh   ri   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cartr]   bob	bob_frameslider
joint_axispinr   Zchild_interframeZchild_pointr:   r   r   r>   )6r   r   r   r   r   r   rw   r.   r   r   rb   r   rd   r   zr   r@   r/   r   r   apply_uniform_gravityr   r   r   rm   r   rs   r1   r
   r   mass_matrixr   r   r   forcingr   rF   r   pos_fromdotr#   r$   r&   r'   r_   r`   rY   r   xreplacevaluesrow_joincol_joinr   r   )r   glmcmpkFqpqcupZucr   r   r   r   r   r   r   subsZupd_expectedZ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   r]   r   r   r   r   r   r   r:   r   r   r<   r   )7r   r   r   r   r   r   rw   r.   r   r   rb   r   rd   r   r   r   r@   r/   r   r   r^   r   r   Zpotential_energyr   r   r   rm   r   r   r   r   r   r   r   r   r   rF   r   r1   r#   r$   r_   r`   Zlam_vecZsolve_multipliersr   r   rs   r   r   r  r   r   r   )r   r  r  r  r  r  r  r  r	  ZqpdZqcdr   r   r   r   r   r   r   ru   r  Zqpdd_expectedZeomsZlam1Zlam1_solZ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)Zpositiver   r]   r   r:   )"r   r   r   r   ra   rb   rc   r   rd   Zset_velr   r   r#   r&   r)   r_   r`   r*   r   r   r	   rm   r   r   r   r   r  r   r   r   r   r   r1   Zauxiliary_eqs)r   r  r\   mur%   r(   rE   r   r  r   r   r  r  r  r  r  r   Zaux_eqsr    r    r!   test_box_on_ground  s>   
 


"
z%TestSystemExamples.test_box_on_groundN)rf   rg   rh   r  r  r  r    r    r    r!   r     s    EGr   ))ri   Zsympy.core.symbolr   Zsympy.core.sympifyr   Z(sympy.functions.elementary.trigonometricr   r   Zsympy.matrices.denser   r   Zsympy.matrices.immutabler   Zsympy.physics.mechanicsr	   r
   r   r   r   r   r   r   r   r   r   r   r   Zsympy.simplify.simplifyr   Zsympy.solvers.solversr   Z_tr`   r%   r?   r(   rE   r   rj   r   r   r    r    r    r!   <module>   s*    <
U   K^