a ijB@s,dZddlZddlZddlZddlZddlmZmZmZm Z m Z m Z m Z ddl Z ddlZGdddejZGdddejZejdejd ejd ejd iZejd d GdddZGdddejZGdddeZddeddddddejdddf ejeejgejfee ejeeejejgejfeeeeeee e ee fee eeeegdfe!e ejeefdddZ"d&eejgejfejejej#e eeeje eje fdddZ$d'eejgejfejejejej#e eeejee ee%e d d!d"Z&d(ejeej#ee d#d$d%Z'dS))z5Nonlinear Least Squares minimization with box bounds.N)CallableListOptionalSequenceTextIOTupleUnionc@seZdZdZdZdZdZdS) VerbosityrN)__name__ __module__ __qualname__SILENTZFINALITERFULLITERrrO/home/ghrups/robot-bench/.venv/lib64/python3.9/site-packages/mujoco/minimize.pyr sr c@s,eZdZeZeZeZeZdS)StatusN) r rrenumautoFACTORIZATION_FAILEDNO_IMPROVEMENTMAX_ITERDX_TOLrrrrr"srzfactorization failed.zinsufficient reduction.zmaximum iterations reached.znorm(dx) < tol.T)frozenc@speZdZUdZejed<ejed<ejed<ejed<dZe ejed<dZ e ejed<dZ e ejed <dS) IterLoga"Log of a single iteration of the non-linear least-squares solver. Attributes: candidate: Value of the decision variable at the beginning this iteration. objective: Value of the objective at the candidate. reduction: Reduction of the objective during this iteration. regularizer: Value of the regularizer used for this iteration. residual: Optional value of the residual at the candidate. jacobian: Optional value of the Jacobian at the candidate. step: Optional change in decision variable during this iteration. candidate objective reduction regularizerNresidualjacobianstep) r rr__doc__npndarray__annotations__float64r"rr#r$rrrrr1s     rc@sDeZdZdZejejejdddZ ejejejdddZ dS) NormahAbstract interface for norm functions, measuring the magnitude of vectors. Key Concepts: * Norm Value: The value of the norm for a given input vector. * Gradient and Hessian: The gradient (first derivative) and Hessian (second derivative) of the norm function with respect to the input vector. Subclasses Must Implement: * `value(self, r: np.ndarray)`: Computes and returns the norm value for the input vector `r`. * `grad_hess(self, r: np.ndarray, proj: np.ndarray)`: Computes and returns both the gradient and Hessian of the norm at `r`, projected onto `proj`. The reason we ask the user to perform the projection themselves is that norm Hessians are often large and sparse, and the "sandwich" projection operator `proj.T @ hess @ proj` can be computed efficiently by taking the specific norm structure into account. )rreturncCsdS)z@Returns the value of the norm at the input vector `y = norm(r)`.Nrselfr+rrrvalue]sz Norm.valuer+projcCsdS)aZComputes the projected gradient and Hessian of the norm at `r`. Args: r: A NumPy column vector (nr x 1). proj: A pre-computed projection matrix (nr x nx). Returns: A tuple containing: * Projected gradient: proj.T @ (d_norm/d_r). * Projected Hessian: proj.T @ (d^2_norm/d_r^2) @ proj. Nr)r.r+r1rrr grad_hessbs zNorm.grad_hessN) r rrr%abcabstractmethodr&r'r)r/r2rrrrr*Hs r*c@s4eZdZdZejdddZejejdddZdS) Quadraticz%Implementation of the quadratic norm.)r+cCsd|j|S)z"Returns the quadratic norm of `r`.?)Titemr-rrrr/uszQuadratic.valuer0cCs|j|}|j|}||fS)aJComputes the projected gradient and Hessian of the quadratic norm at `r`. Args: r: A NumPy column vector (nr x 1). proj: A pre-computed projection matrix (nr x nx). Returns: A tuple containing: * Projected gradient: `proj.T @ r`. * Projected Hessian: `proj.T @ proj`. )r7)r.r+r1gradhessrrrr2ys  zQuadratic.grad_hessN)r rrr%r&r'r/r2rrrrr5rsr5gư>gחAg&$?dF)x0r"boundsr#normepsmu_minmu_max mu_factortolmax_iterverboseoutput iter_callbackcheck_derivativesr,c<s$t}t| j} d}tj}d}|j}|tj |df}t |df}t |df}t ||df}t|}td}d}g}d}d}d}d}dkrt dfdd}fd d } |d urd nt |}|d urt |d krt d |dj|ks|dj|krt dtt|drHtt|dsPt dt|d|dkspt d|d |d|d<|d |d|d<tj||d|d|dtt|st dt}!||}"|"}#|t|!7}|d7}|"jtjkrt dt| D]}|tjkr.q||"}$t}!|d urrt|||"|||\}%}|t|!7}nH|||"}%|t|!7}|d7}|dkr|rt|||"|%|||| }|dkr|rt|tst|"||| ||"|%\}&}'|d urd n |d|}(|d urd n |d|})d}*d}+|*dkr4d},|,st||d |'|t||&|(|)}-|-dkrxd},n"||krtj}qn | |\}}q>|tjkrq4||}t}!||}#|t|!7}|d7}||#}.|$|.}+|+||&j|}*|*dkr0||kr$tj}q4| |\}}q0|tjkrFq|&j|d|j|'| }/d}0|/dkr| tj jkr|/dkrt!d| dn|/dkrt!d| dn|+|/}0tj"#|}1| tj$jkr2|dkrt%|ntj& }2d|dd|$dd|2d d!|0d"d#|1d"d$|+d" }3t!|3| dt'||$|+|d%}4| tj(jkrbt)j*|4|"|%|d&}4|+|4| d ur~| ||1| krtj,}q|0d'kr|||\}}n|0d(kr| |\}}|}|#}"q||"}5td}6t'||5|6|d%}4|+|4| d ur| || tj jkrd)|d*}3|3t-|7}3|3d+|5d,d-|d.7}3|dkrh|3d/|d.7}3t!|3| dt|}7|7dkrd0||7}8|7dkrdnd1}9|7dkrd2nd3}:d4|9|7d5|:}3|3d6|8d5d77}3|dkrd0||7};|3d8|;d5d77}3t!|3| d| |j.|fS)9aNonlinear Least Squares minimization with box bounds. Args: x0: Initial guess residual: Vectorized function returning the residual for 1 or more points. bounds: Optional pair of lower and upper bounds on the solution. jacobian: Optional function that returns Jacobian of the residual at a given point and residual. If not given, `residual` will be finite-differenced. norm: Norm object returning norm scalar or its projected gradient and Hessian. See Norm class for detailed documentation. eps: Perurbation used for automatic finite-differencing. mu_min: Minimum value of the regularizer. mu_max: Maximum value of the regularizer. mu_factor: Factor for increasing or decreasing the regularizer. tol: Termination tolerance on the step size. max_iter: Maximum number of iterations. verbose: Verbosity level. output: Optional file or StringIO to which to print messages. iter_callback: Optional iteration callback, takes trace argument. check_derivatives: Compare user-defined Jacobian and norm against fin-diff. Returns: x: best solution found trace: sequence of solution iterates. g{Gz?rr zmu_factor must be > 1.cs8dd|}||kr dn||}|d7}||fS)Nr r rJr)mun_reducZdmurBr@rr decrease_musz"least_squares..decrease_mucst|}d}||fS)Nr)max)rKrLrMrr increase_musz"least_squares..increase_muNr zbounds must have 2 elements.z%bounds must have the same size as x0.zbounds must be finite.z)bounds[0] must be smaller than bounds[1].)outzx0 must be finite.z-residual function must return float64 arrays.FTr6z-Zero expected reduction: exact minimum found?filez.Negative expected reduction: should not occur.ziter: z<3dz y: z<9.4gz log10mu: z>4.1fz ratio: z<7.2gz dx: z reduction: )rrr r!)r"r#r$g?g?zTerminated after z iterations: z y: <.4gz, Residual evals: dz, Jacobian evals: r;ismsz total time z<.1fz of which residual %z Jacobian )/timer r/rrsizeZastyper&r)ZreshapeZzeros ValueErrorlistlenallisfiniteZclipZdtyperange jacobian_fdcheck_jacobian isinstancer5 check_normr2mujocoZ mju_boxQPeyerr7r8rrprintlinalgr>rlog10infrr dataclassesreplaceappendr_STATUS_MESSAGEshape)r?r@rArBrCrDrErFrGrHZ t_start_totalZ armijo_c1statusinxZxnewZdxZscratchrKrLtracen_resZn_jacZt_resZt_jacrNrPZt_startr+Zrnewyjacr9r:ZdlowerZdupperZarmijor Z factorizableZn_freeZynewZexpected_reductionZreduction_ratioZdx_normZlogmumessagelogZyfinalredZ time_totalZ r_percentZ time_scaleZ time_unitsZ jac_percentrrMr least_squaress4*      ,            "                         r|)r"rtr+r?rvr=r,c Csx|j}|dur|t|}n,d|d|d}t||k| |}|t|} || } | ||} | ||fS)aFinite-difference Jacobian of a residual function. Args: residual: vectorized function that returns the residual of a vector array. x: point at which to evaluate the Jacobian. r: residual at x. eps: finite-difference step size. n_res: number or residual evaluations so far. bounds: optional pair of lower and upper bounds. Returns: jac: Jacobian of the residual at x. n_res: updated number of residual evaluations (add x.size). Nr6r r)r[r&ZoneswhereflattenZdiag) r"rtr+r?rvr=rsZeps_vecmidxhZrhrxrrrrbs rbJacobian) r"rtr+rxr?rvr=rFnamer,c Cs~t||||||\} }t|t| d} t|| | } t| dkrftd|dtd|d|d|S)aCCheck user-provided Jacobian against internal finite-differencing. Args: residual: vectorized function that returns the residual of a vector array. x: point at which the r and jac were evaluated. r: residual at x. jac: Jacobian at x. eps: finite-difference step size. n_res: number or residual evaluations so far. bounds: optional pair of lower and upper bounds. output: Optional file or StringIO to which to print messages. name: Optional name of the function being tested. Returns: n_res: updated number of residual evaluations. g:0yE>gh㈵>zUser-provided zC does not match finite-differences to a relative tolerance of 1e-5.z matches finite-differences.rS)rbr&abssumanyr\rh) r"rtr+rxr?rvr=rFrZjac_fddenomZrel_diffrrrrcs  rc)r+r>r?rFc st|}t|j|\}}t||jsBtdt tj |dkrzt tj |}td|dfdd}t ||||j|dd|d fd d } t | ||||dd|d dS) a3Check user-provided norm against internal finite-differencing. Args: r: residual vector. norm: Norm function returning either the norm scalar or its gradient and Gauss-Newton Hessian. eps: finite-difference step size. output: Optional file or StringIO to which to print messages. z,User-provided norm Hessian is not symmetric.rzKUser-provided norm Hessian is not positive definite. Minimum eigenvalue is rUcs(fddtjdD}t|S)Nc s0g|](}tdd||dfqS)Nr )r& atleast_2dr/.0rr)r>vrr sz0check_norm..norm_vec..r rarpr&Zhstack)rZnorms)r>rrnorm_vecs  zcheck_norm..norm_vecNz norm gradientcs*fddtjdD}t|S)Nc s0g|](}dd||dfdqS)Nr r)r2r)rgr>rrrrsz0check_norm..grad_vec..r r)rZ gradientsrgr>rrgrad_vec s zcheck_norm..grad_vecz norm Hessian)r&rr/rgr[r2Zallcloser7r\rriZeigvalsminrc) r+r>r?rFrsZn_gZn_hZh_minrrrrrres   re)N)NNr)N)(r%r3rlrrZtypingrrrrrrrrfnumpyr&Enumr rrrrrro dataclassrABCr*r5rr'floatintboolr|r)rbstrrcrerrrrs$ *     )  +