U
    L?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    a/var/www/html/venv/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_system_class.py_empty_system_setup   s    z"TestSystemBase._empty_system_setupr    c                 C   sx   d}d}|D ](}||krt | j|d d  g kstq|D ] }||kr:t | j|dks:tq:d|krt| jjd kstt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   AssertionErrorr1   )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   s   | j dd d S )NTrM   rN   r   r    r    r!   _filled_system_setupB   s    z#TestSystemBase._filled_system_setupc                 C   s   | j dd d S )NFrO   rP   r   r    r    r!   _filled_system_setup_no_speedsF   s    z-TestSystemBase._filled_system_setup_no_speedsc                 C   sj  d|ks(| j jd d  td d ks(td|ksN| j jd d  td gksNtd|ksv| j jd d  td d ksvtd|ks| j jd d  td d kstd|ks| j jd d  tdd kstd	|ks| j jd d  td d kstd
|ks| j jd d  t	d d kstd|ks^| j j
d d  dd ttd d td d D ks^td|ks| j jd d  td td  td  gkstd|ks| j jd d  td td  td  gkstd|ks"| j jd d  td td  td  td td  td  gks"td|ksD| j jt| jksDtd|ksf| j jt| jksftd S )Nr#   r=   r$   r%   r?   r&   r;   r'   r(   r)   r*   c                 S   s   g | ]\}}|| qS r    r    .0ZuiZqdir    r    r!   
<listcomp>R   s    z7TestSystemBase._filled_system_check.<locals>.<listcomp>r+   r   r   r,   rJ   r.   r/   )r   r#   r%   r3   r$   r&   r(   r'   r)   rF   r*   zipr@   r+   r,   rJ   r.   tupler/   )r   r4   r    r    r!   _filled_system_checkJ   s8    (&((((,
(
(
 

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"   rY   r    r    r!   _moving_point_massa   s    z!TestSystemBase._moving_point_massN)r    )T)r    )__name__
__module____qualname__pytestZfixturer"   r6   rN   rQ   rR   rX   rc   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dddg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dddg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d/d0ed ed  fi fd1d2ed ed  fi fd3d4ed ed  fi fd5d6ed7fi fd8d9ee	d:ed;jfi fd<d=eed>ed;jed?fi fgd@dA ZejdBd)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/d0dCefd1d2dCefd3d4dCefd5d6edefd8d9edefd<d=edefgdDdE ZejdFdGi dHdI eeddJ eddJ D fedJ edJ  ed ed  fi dKdI eeddL eddL D fgdMdN Z ejded ed  edJ edJ  fi fed ed   edJ edJ  fi fed ed  edJ edJ  gi fgdOdP Z!ejdQdGi ed ed  ed  gfedJ ed  ed ed  fi ed ed  ed  edJ ed  ed ed  gfgdRdS Z"ejded ed  ed  edJ ed  fi fed ed  ed   edJ ed  fi fed ed  edJ ed  fi fgdTdU Z#ejdQdGi ed ed  ed  gfedJ ed  ed ed  fi ed ed  ed  edJ ed  ed ed  gfgdVdW Z$ejded ed  ed  edJ ed  fi fed ed  ed   edJ ed  fi fed ed  edJ ed  fi fed ed  edJ ed  gi fgdXdY Z%ejdZg 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_d` Z(dadb Z)dcdd Z*dedf Z+dgdh Z,ejdidjdkdlgdmdn Z-ejdodpdqdlgdrds Z.ejdte/e0gdudv Z1ejdwi e2dxdgdedyggfdzdie2ddgdedyggfgd{d| Z3ejd}i e2ddgdedygge2ed 4e5dgfgd~d Z6ejde/ddiefe0ddiefe/d5g iefe/dg iefe0d5g iefe0ddiefgdd Z7dS )
TestSystemc                 C   s   |    | j  d S N)r6   r   validate_system)r   r"   r    r    r!   test_empty_systeml   s    zTestSystem.test_empty_systemc                 C   s   |    | j  d S ri   )rX   r   rj   r   rQ   r    r    r!   test_filled_systemp   s    zTestSystem.test_filled_systemr   Nr   c                 C   s2  |d kr|d krt  | _nt ||| _|d krB| jjjdksRtn| jj|ksRt|d krn| jjjdks~tn| jj|ks~t|   t| jjt	stt| jj
t	stt| jjt	stt| jjt	stt| jjt	stt| jjt	stt| jjt	stt| jjt	stt| jjt	s.td S )NZinertial_pointZinertial_frame)r   r   r   namer3   r   r6   
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&t| jj|jks8t| jdd |f| j_	d S )Nbody)r.   r4   )
r   r   from_newtonianr   r   r`   r3   r   r6   r.   )r   rbr    r    r!   test_from_newtonian_rigid_body   s    z)TestSystem.test_from_newtonian_rigid_bodyc              	   C   s,   t d}tt t| W 5 Q R X d S )NZparticle)r   rg   raises	TypeErrorr   rs   )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>   TFr   r;   r   c                 C   s   | j j|| | j jd d  |ks&t| j jd d  |ks>t| j jd d  |ksVt| jdd || j _|| j _| j jd d  |kst| j jd d  |kst| j jd d  |kst| jdd d S )N)r#   r$   r%   rr   )r   rB   r#   r3   r$   r%   r6   )r   r"   argskwargsZ	exp_q_indZ	exp_q_depZexp_qr    r    r!   test_coordinates   s    
zTestSystem.test_coordinatesfuncrB   rC   zargs, kwargs   ac              	   C   s4   t t t| j||| W 5 Q R X |   d S ri   )rg   rv   
ValueErrorr2   r   rX   )r   rQ   r}   rz   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   s   | j j|| | j jd d  |ks&t| j jd d  |ks>t| j jd d  |ksVt| jdd || j _|| j _| j jd d  |kst| j jd d  |kst| j jd d  |kst| jdd d S )N)r&   r'   r(   rr   )r   rC   r&   r3   r'   r(   r6   )r   r"   rz   r{   Z	exp_u_indZ	exp_u_depZexp_ur    r    r!   test_speeds   s    
zTestSystem.test_speedszargs, kwargs, exp_u_auxc                 C   sb   | j j|| | j jd d  |ks&t| jdd || j _| j jd d  |ksRt| jdd d S )N)r)   rr   )r   rE   r)   r3   r6   )r   r"   rz   r{   Z	exp_u_auxr    r    r!   test_auxiliary_speeds   s    z TestSystem.test_auxiliary_speedsc              	   C   s0   t t | jj|| W 5 Q R X |   d S ri   )rg   rv   r   r   rE   rX   r   rQ   rz   r{   r    r    r!   test_auxiliary_invalid   s    z!TestSystem.test_auxiliary_invalidzprop, add_func, args, kwargsr#   r$   r&   r'   r)   rE   r*   rD   r+   rG   r,   rH   r.   r_   rq   r-   	add_loadsPNr0   add_actuatorsTAc                 C   s   t | j|d |ddf}|dkr(|d7 }| j|d tt| j|d d  g ksTtt| j||| tt| j|d d  t|kstd S )Nr    r%   r(   )r+   r,   rJ   rr   )setattrr   rX   listr2   r3   )r   rQ   propadd_funcrz   r{   r4   r    r    r!   test_add_after_reset   s    
 zTestSystem.test_add_after_resetzprop, add_func, value, error   c              	   C   sV   t | t| j|| W 5 Q R X t | t| j|| W 5 Q R X |   d S ri   )rg   rv   r2   r   r   rX   )r   rQ   r   r   valueerrorr    r    r!   test_type_error  s
    zTestSystem.test_type_errorzargs, kwargs, exp_kdesr    c                 C   s   g | ]\}}|| qS r    r    rS   r    r    r!   rU   #  s     zTestSystem.<listcomp>r?   c                 C   s   g | ]\}}|| qS r    r    rS   r    r    r!   rU   %  s        c                 C   sb   | j j|| | jdd | j jd d  |ks2t|| j _| jdd | j jd d  |ks^td S )N)r*   rr   )r   rD   rX   r*   r3   )r   rQ   rz   r{   Zexp_kdesr    r    r!   	test_kdes"  s    zTestSystem.test_kdesc              	   C   s0   t t | jj|| W 5 Q R X |   d S ri   )rg   rv   r   r   rD   rX   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|| | j|d | jjd d  |ksJt| jjd d  |ksbt|| j_| j|d | jjd d  |kst| jjd d  |kstd S )N)r+   rJ   c                 S   s   g | ]}| tqS r    )r]   r^   )rT   cr    r    r!   rU   C  s     z9TestSystem.test_holonomic_constraints.<locals>.<listcomp>rr   )rL   r   rG   rX   r+   r3   rJ   r   rQ   rz   r{   Zexp_conr4   Zexp_vel_conr    r    r!   test_holonomic_constraints;  s    z%TestSystem.test_holonomic_constraintsc              	   C   s0   t t | jj|| W 5 Q R X |   d S ri   )rg   rv   r   r   rG   rX   r   r    r    r!   "test_holonomic_constraints_invalidO  s    z-TestSystem.test_holonomic_constraints_invalidc                 C   s   d}| j d t| j | }| jj|| | j|d | jjd d  |ksNt| jjd d  |ksft|| j_| j|d | jjd d  |kst| jjd d  |kstd S )N)r,   rJ   rr   )	rK   lenrI   r   rH   rX   r,   r3   rJ   r   r    r    r!   test_nonholonomic_constraintsZ  s    z(TestSystem.test_nonholonomic_constraintsc              	   C   s0   t t | jj|| W 5 Q R X |   d S ri   )rg   rv   r   r   rH   rX   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,td S )Nr   rr   )r   rJ   rX   r3   )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sFtd | j_|   d S )Nr=   r;   r   rr   )r@   r   rJ   rX   r3   rl   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j||fksLtj  jjj|| fksttg j_jjdkst j_jj fksttdtt	fdd tt
 fd	d tt	 ||| fj_W 5 Q R X jj fkstd S )
Nrb1rb2p1p2r    symbc                      s    j S ri   r   r_   r    )r   r   r    r!   <lambda>      z(TestSystem.test_bodies.<locals>.<lambda>c                      s   j  S ri   r   r    )r   r   r    r!   r     r   )r   r   r   r_   r.   r3   r   rg   rv   rw   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st
||jfg_	j	||jffkst
tt fdd tt   jf_	W 5 Q R X j	||jffkst
d S )	Nr   r   r   )r   mc1r   c                      s      j jfS ri   )r   rb   yr    r   r   r    r!   r     r   z+TestSystem.test_add_loads.<locals>.<lambda>)r   r   r   r   r   r   r   rb   r	   r-   r3   rg   rv   r   rw   )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sXt|j	dksft|f|_|j|fks~td S )Nr   r   ZT1ZT2r    )
r   r   r   r   rb   r   r   r0   r3   r-   )r   r   r   r   Zact1Zact2r    r    r!   test_add_actuators  s    
zTestSystem.test_add_actuatorsc                    st  t d\}}}}}}}tdtd\}}	}
}td|||}td||	|| td|	|
||}td|
|||t}t | j	|fkst
j|fkst
jt|gkst
jt|gkst
jt||t gkst
|
 | ||t  | j	||fks0t
j||
|	fksHt
jt||gks`t
jt||gksxt
jt||t ||t gkst
||t     j	|| fkst
j||
|	fkst
jt|||gkst
jt|||gks&t
jt||t ||t ||t  gks`t
| j	|| |fkst
j||
|	|fkst
jt||||gkst
jt||||tgkst
jt||t ||t ||t  gkst
jd d  g ks,t
jd d  g ksDt
tt fdd	 ttfd
d	 d S )Nz	q1:5 u1:4zrb1:6r7   r9   r:   r<   J_lagc                      s
     S ri   rA   r    )r:   r   r    r!   r     r   z,TestSystem.test_add_joints.<locals>.<lambda>c                      s
     S ri   r   r    )r   r   r    r!   r     r   )r   r   r   r   r   r]   r^   r   rA   r/   r3   r.   r#   r   r&   r*   r_   rB   rD   r$   r'   rg   rv   r   rw   )r   q1q2Zq3Zq4u1u2u3r   rb3Zrb4Zrb5r9   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,t| jdd d| j_d| j_| j | j_ |   d S )Nr   )r/   rr   r    )r/   r   r3   rX   r#   r&   rl   r    r    r!   test_joints_setter  s    
zTestSystem.test_joints_setterzname, joint_index)r9   r   )r:   r   )Znot_existingNc                 C   s8   | j |}|d kr"|d ks4tn|| j| ks4td S ri   )r   	get_jointr3   r/   )r   rQ   rn   Zjoint_indexZjointr    r    r!   test_get_joint  s    zTestSystem.test_get_jointzname, body_index)r   r   )r   r;   c                 C   s8   | j |}|d kr"|d ks4tn|| j| ks4td S ri   )r   get_bodyr3   r.   )r   rQ   rn   Z
body_indexrq   r    r    r!   test_get_body  s    zTestSystem.test_get_bodyr1   c                 C   s4   G dd d|}| j j|d t| j j|s0td S )Nc                   @   s   e Zd ZdS )z:TestSystem.test_form_eoms_calls_subclass.<locals>.MyMethodN)rd   re   rf   r    r    r    r!   MyMethod  s   r   )r1   )r   	form_eomsro   r1   r3   )r   rc   r1   r   r    r    r!   test_form_eoms_calls_subclass  s    z(TestSystem.test_form_eoms_calls_subclasszkwargs, expectedrZ   Zexplicit_kinematicsc                 C   s"   | j jf | | j j|kstd S ri   )r   r   mass_matrix_fullr3   )r   rc   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f dti| | j j|ks&t| j j|ks6td S )Nr1   )r   r   r   r   r3   forcing_full)r   rc   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   sv   t d | j_tdtdd}| j| |j| jjt d | jj	  t
| | jjf d|i| W 5 Q R X d S )Nr   rY   rZ   r[   r1   )r%   r   r#   r   r   r_   r`   ra   r   rb   rg   rv   r   )r   r"   r1   r{   r   rY   r    r    r!   test_form_eoms_kwargs_errors!  s    
z'TestSystem.test_form_eoms_kwargs_errors)8rd   re   rf   rk   rm   rg   markparametrizer   r   rp   ru   ry   r%   r|   r(   r   r   r   rF   r   r   r@   r   r	   rb   r   r   r   rw   r   rV   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!   rh   k   s`  $((,
$((,


    



( "

"$"
 4
*,"
 4
*,""
0>
,	





rh   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   s>   | j |d | j| tt | j| W 5 Q R X d S )NrO   )rN   r   rj   rg   rv   r   )r   Zvalid_methodZinvalid_methodrM   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                      s   j  S ri   r   rj   r    methodr   r    r!   r   F  r   zBTestValidateSystem.test_missing_joint_coordinate.<locals>.<lambda>)rN   r#   r   r&   r*   rg   rv   r   r   r   rM   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                      s
    j  S ri   r   r    r   r    r!   r   L  r   z=TestValidateSystem.test_missing_joint_speed.<locals>.<lambda>)r#   r   r&   r*   rg   rv   r   rl   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                      s
    j  S ri   r   r    r   r    r!   r   P  r   z<TestValidateSystem.test_missing_joint_kdes.<locals>.<lambda>)r*   r   rg   rv   r   rl   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   rj   rl   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                      s   j  S ri   r   r    r   r    r!   r   ]  r   zFTestValidateSystem.test_missing_holonomic_constraint.<locals>.<lambda>)rN   r   r+   rL   r&   r'   r,   rg   rv   r   r$   r#   rj   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                      s
    j  S ri   r   r    r   r    r!   r   d  r   zITestValidateSystem.test_missing_nonholonomic_constraint.<locals>.<lambda>r   r   )r   r,   rg   rv   r   r'   r&   rj   rl   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                      s
    j  S ri   r   r    r   r    r!   r   r  r   zFTestValidateSystem.test_number_of_coordinates_speeds.<locals>.<lambda>)
r&   r(   r   r*   r@   rj   r#   rg   rv   r   rl   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                      s
    j  S ri   r   r    r   r    r!   r   w  r   z8TestValidateSystem.test_number_of_kdes.<locals>.<lambda>r;   r   c                      s
    j  S ri   r   r    r   r    r!   r   y  r   )r*   r   rg   rv   r   r(   r@   rl   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   rl   r    r    r!   test_duplicates{  s    z"TestValidateSystem.test_duplicatesc              	   C   s   t d t| j | j_tt | jt W 5 Q R X g | j_| jt t	| j_
tt | jt W 5 Q R X g | j_
| jt | jtdtdtd g | j_tt | jt W 5 Q R X d S )NZJuZrbu1Zrbu2)r(   r   r&   r   rg   rv   r   rj   r   rF   r)   rA   r   r   )r   rR   r    r    r!   test_speeds_in_lagrange  s     z*TestValidateSystem.test_speeds_in_lagrangeN)rd   re   rf   rg   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sft|j	|j	ksvt|j
|jksttd||||
|jd	}td
||||	|j|||j d}||| |j||fkst|d|kst|d|kst|| |j  ||j||j f |t|| |j|| |  |  t|jtsbtt|jt || || t!| g|| t!| ||d  gg t"ddkstt|j#t || |	d  t$| | g| | | t$| ||  gg t"ddkst|%t&|j'|j(|j |jd ks@t|| |_)|_*|	|
 |_+|_,|  || t$| |
| t!| |	 |
-t.||	d t$| |	-t.t!|   i}| | t$| || |  || t$d|  |	d  d  || t$d|  |	d  d  |t!|  ||t!|d  |t$|d     }t/t0| 1||	-t.2 d }t|| dks~tt|jtst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}|3t"dd4t"dd3|}|4|}t|j| t"ddkstt|j#| t"ddkstt|j5| t"ddkstt|j6| t"ddks$t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?   )7r   r   r   r   r   r   rs   r.   r3   r   r   r`   r   rb   r   zr   rA   r/   r   r   apply_uniform_gravityr   r   r   rj   r   ro   r1   r
   r   mass_matrixr   r   r   forcingr   rG   r   pos_fromdotr#   r$   r&   r'   r]   r^   rW   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spt|j	|j	kst|j
|jksttd
||||
|jd}td||||	|j|||j d}||| |j||fkst|d
|kst|d|ks
t|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stt|j$t!|| |	d  t%| | g| | | t%| ||  gg t#ddks,t|&t'|j|j|j |j(d ks\t|| |_)|_*|| 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 0||i0||	+t,d }t|| dkstt1|j(tstt!|d | || t"| | t"| g|| t"| || dgg}t!| | | t%| ||  g|| t%| |	d  | gg}t2d3t#dd4t#dd3|4t!|t"| ddgj5}t!|	|
g|d d   |t%| |	d  g }t|j | t#ddkstt|j$| t#ddkstt|j6| t#ddkstt|j7| t#ddks*td S )Nr   zF qp qczqp qcr   r   r   r[   r   r   r   r   r   r   r;   r   r   r=   r~   )8r   r   r   r   r   r   rs   r.   r3   r   r   r`   r   rb   r   r   r   rA   r/   r   r   r\   r   r   Zpotential_energyr   r   r   rj   r   r   r   r   r   r   r   r   r   rG   r   r1   r#   r$   r]   r^   Zlam_vecZsolve_multipliersr   r   ro   r   r   r   r   r   r   )r   r   r   r   r   r   r   r   r   ZqpdZqcdr   r   r   r   r   r   r   rq   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stt|
j| tddkstt|
j| td	d	kstt|
j | td	dkstt|
j!j"| tddkstd S )
Nzg m muzq u uazN FT)Zpositiver   r[   r   r;   )#r   r   r   r   r_   r`   ra   r   rb   Zset_velr   r   r#   r&   r)   r]   r^   r*   r   r   r	   rj   r   r   r   r   r   r   r   r3   r   r   r   r1   Zauxiliary_eqs)r   r   rZ   mur%   r(   rF   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)rd   re   rf   r  r  r  r    r    r    r!   r     s   EGr   ))rg   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(   rF   r   rh   r   r   r    r    r    r!   <module>   s(   <
U   K^