a
    ij$                     @   sR   d Z ddlZddlmZ ddlmZ ddlZG dd dejZ	e
dkrNe  dS )zTests for minimize.py.    N)absltest)minimizec                   @   s|   e Zd ZddddZddddZddddZddd	d
ZddddZddddZddddZ	ddddZ
dS )MinimizeTestN)returnc                 C   sZ   dd }t  }td}tj|||d\}}td}tj|| | d|	  d S )Nc              	   S   sB   t d| dd d f  d| dd d f | dd d f d   gS N   r   
      npstackx r   T/home/ghrups/robot-bench/.venv/lib64/python3.9/site-packages/mujoco/minimize_test.pyresidual   s    z)MinimizeTest.test_basic.<locals>.residual        r   output      ?r   norm(dx) < tol
ioStringIOr   arrayr   least_squarestestingassert_array_almost_equalassertIngetvalueselfr   outx0r   _
expected_xr   r   r   
test_basic   s    

zMinimizeTest.test_basicc                 C   sj   dd }t  }td}tj|||d\}}td}tj|| | d|	  | d|	  d S )Nc              	   S   sB   t d| dd d f  d| dd d f | dd d f d   gS r   r
   r   r   r   r   r   &   s    z4MinimizeTest.test_start_at_minimum.<locals>.residualr   r   r   zexact minimum foundr   r"   r   r   r   test_start_at_minimum%   s    

z"MinimizeTest.test_start_at_minimumc           	      C   s   dd }dd }t d}t }tj||||dd\}}t d}t j|| | d	|	  | d
|	  dd }| 
td$ tj||||dd W d    n1 s0    Y  d S )Nc              	   S   sB   t d| dd d f  d| dd d f | dd d f d   gS r   r
   r   r   r   r   r   2   s    z0MinimizeTest.test_jac_callback.<locals>.residualc                 S   s    ~t ddgd| d  dggS )Nr   r   r   r   r   r   r   rr   r   r   jacobian5   s    z0MinimizeTest.test_jac_callback.<locals>.jacobianr   T)r0   r   check_derivativesr   r   zJacobian matchesc                 S   s    ~t ddgd| d  dggS )Nr*   r   r+   r,      r-   r.   r   r   r   bad_jacobianD   s    z4MinimizeTest.test_jac_callback.<locals>.bad_jacobianz\bJacobian does not match\b)r   r   r   r   r   r   r   r   r    r!   assertRaisesRegex
ValueError)	r#   r   r0   r%   r$   r   r&   r'   r3   r   r   r   test_jac_callback1   s(    



zMinimizeTest.test_jac_callbackc                    sp   d  fdd}t  }t }tj||d|d | d|  t||\}}t d}t j	
|| d S )N   c                    sB    fddt d D } fddt d D }t|| S )Nc                    s    g | ]}d  |ddf  qS )r   Nr   .0ir   r   r   
<listcomp>U       z@MinimizeTest.test_max_iter.<locals>.residual.<locals>.<listcomp>r   c                    s8   g | ]0}d  |ddf  |d ddf d   qS )r   Nr   r	   r   r8   r   r   r   r;   V   r<   )ranger   Zasarray)r   Zres0Zres1dimr   r   r   T   s    z,MinimizeTest.test_max_iter.<locals>.residual)Zmax_iterr   zmaximum iterations)r   Zzerosr   r   r   r   r    r!   Zonesr   r   )r#   r   r%   r$   r   r&   r'   r   r>   r   test_max_iterQ   s    

zMinimizeTest.test_max_iterc                 C   sf  dd }t  }td}td}tdtdgtdtdgtdtd	gd
}tj|||d |d\}}tj|| | d|	  |
 D ]}t  }tj||||tjjd\}}	| d|	  |	d jj|	d j }
t|D ]h\}}||d | kr"| |
| d q||d | krF| |
| d q| j|
|  ddd qqd S )Nc              	   S   sB   t d| dd d f  d| dd d f | dd d f d   gS r   r
   r   r   r   r   r   e   s    z*MinimizeTest.test_bounds.<locals>.residualr   r   )       rA   )       @rB   )rA   rB   )      ?g      @)rC   rB   )inboundsZonlowerZonupperrD   boundsr   r   )rF   r   verbosez < tolr   r      )Zplaces)r   r   r   r   r   r   r   r   r    r!   valuesZ	VerbosityZFULLITERr0   Tr   	enumerateZassertGreaterZ
assertLessZassertAlmostEqualitem)r#   r   r$   r%   r'   Zbounds_typesr   r&   rF   tracegradr:   xir   r   r   test_boundsd   s<    



zMinimizeTest.test_boundsc              	   C   s   dd }t  }td}g dtdtdgtdtddtjfgtdtd	gg}|D ]@}| t" tj||||d
 W d    qf1 s0    Y  qfd S )Nc                 S   s*   t d| d  d| d | d d   gS r   r-   r   r   r   r   r      s    z.MinimizeTest.test_bad_bounds.<locals>.residualr   )r   r   r	   )rH   r	   r   )rC      rI   rC   rR   )rR      rE   )	r   r   r   r   infZassertRaisesr5   r   r   )r#   r   r$   r%   Z
bad_boundsrF   r   r   r   test_bad_bounds   s    
zMinimizeTest.test_bad_boundsc                    sh   dd }t    fdd}td}tj|| |d\}}td}tj|| | d 	  d S )	Nc              	   S   sB   t d| dd d f  d| dd d f | dd d f d   gS r   r
   r   r   r   r   r      s    z1MinimizeTest.test_iter_callback.<locals>.residualc                    s   t dt|  d d d S )NzHello iteration !)file)printlen)rN   r$   r   r   iter_callback   s    z6MinimizeTest.test_iter_callback.<locals>.iter_callbackr   )r   r\   r   zHello iteration 3!r   )r#   r   r\   r%   r   r&   r'   r   r[   r   test_iter_callback   s    


zMinimizeTest.test_iter_callbackc                    s   dd }d G  fdddt j}t }td}t j||| |dd\}}td	}tj|| | 	d
|
  | 	d|
  | 	d|
  G  fdddt j}| td& t j||| |dd W d    n1 s0    Y  G  fdddt j}	| td& t j|||	 |dd W d    n1 s>0    Y  G  fdddt j}
| td& t j|||
 |dd W d    n1 s0    Y  G  fdddt j}| td& t j||| |dd W d    n1 s0    Y  d S )Nc              	   S   sB   t d| dd d f  d| dd d f | dd d f d   gS r   r
   r   r   r   r   r      s    z(MinimizeTest.test_norm.<locals>.residualg{Gz?c                       s$   e Zd Z fddZ fddZdS )z(MinimizeTest.test_norm.<locals>.SmoothL2c                    s    t |j|        S Nr   sqrtrK   rM   r#   r/   pr   r   value   s    z.MinimizeTest.test_norm.<locals>.SmoothL2.valuec                    s^   t |j|      }|| }|j| }t |j||j  | }|j| | }||fS r^   r   r`   rK   rM   eyesizer#   r/   ZprojsZy_rrO   Zy_rrZhessrb   r   r   	grad_hess   s    
z2MinimizeTest.test_norm.<locals>.SmoothL2.grad_hessN__name__
__module____qualname__rd   rj   r   rb   r   r   SmoothL2   s   ro   r   T)Znormr   r1   r   r   z#User-provided norm gradient matchesz"User-provided norm Hessian matchesc                       s$   e Zd Z fddZ fddZdS )z/MinimizeTest.test_norm.<locals>.SmoothL2BadGradc                    s    t |j|        S r^   r_   ra   rb   r   r   rd      s    z5MinimizeTest.test_norm.<locals>.SmoothL2BadGrad.valuec                    sb   t |j|      }|| }|j|d  }t |j||j  | }|j| | }||fS )NgMbP?re   rh   rb   r   r   rj      s    z9MinimizeTest.test_norm.<locals>.SmoothL2BadGrad.grad_hessNrk   r   rb   r   r   SmoothL2BadGrad   s   rp   z\bgradient does not match\bc                       s$   e Zd Z fddZ fddZdS )z/MinimizeTest.test_norm.<locals>.SmoothL2BadHessc                    s    t |j|        S r^   r_   ra   rb   r   r   rd      s    z5MinimizeTest.test_norm.<locals>.SmoothL2BadHess.valuec                    sb   t |j|      }|| }|j| }dt |j ||j  | }|j| | }||fS )Ngjt?re   rh   rb   r   r   rj      s    
z9MinimizeTest.test_norm.<locals>.SmoothL2BadHess.grad_hessNrk   r   rb   r   r   SmoothL2BadHess   s   rq   z\bHessian does not match\bc                       s$   e Zd Z fddZ fddZdS )z0MinimizeTest.test_norm.<locals>.SmoothL2AsymHessc                    s    t |j|        S r^   r_   ra   rb   r   r   rd      s    z6MinimizeTest.test_norm.<locals>.SmoothL2AsymHess.valuec                    sb   t |j|      }|| }|j| }t |j|d |j  | }|j| | }||fS )Ng-C6?re   rh   rb   r   r   rj      s    
z:MinimizeTest.test_norm.<locals>.SmoothL2AsymHess.grad_hessNrk   r   rb   r   r   SmoothL2AsymHess   s   rr   z\bnot symmetric\bc                       s$   e Zd Z fddZ fddZdS )z/MinimizeTest.test_norm.<locals>.SmoothL2NegHessc                    s    t |j|        S r^   r_   ra   rb   r   r   rd     s    z5MinimizeTest.test_norm.<locals>.SmoothL2NegHess.valuec                    s`   t |j|      }|| }|j| }t |j||j   | }|j| | }||fS r^   re   rh   rb   r   r   rj     s    
z9MinimizeTest.test_norm.<locals>.SmoothL2NegHess.grad_hessNrk   r   rb   r   r   SmoothL2NegHess  s   rs   z\bnot positive definite\b)r   ZNormr   r   r   r   r   r   r   r    r!   r4   r5   )r#   r   ro   r$   r%   r   r&   r'   rp   rq   rr   rs   r   rb   r   	test_norm   sb    


$&&zMinimizeTest.test_norm)rl   rm   rn   r(   r)   r6   r@   rQ   rV   r]   rt   r   r   r   r   r      s    *r   __main__)__doc__r   Zabsl.testingr   Zmujocor   numpyr   ZTestCaser   rl   mainr   r   r   r   <module>   s     