packages = ['numpy']
The real-time MPC for a tracking task subject to box constraints video for the submitted paper entitled "Projection-based first-order optimization solver for robotics" is below:
The examples below were created as extensions of the toolbox Robotics Codes from Scratch.


We would like the robot's end-effector to stay inside a disk shaped region defined by its center $\nd x_c$:x_c and its radius $r$:param.radius ($\mathcal{C}_{\nd p}$) while respecting joint limits ($\mathcal{C}_{\nd q}$)

$\mathcal{C}_{\nd q}=\{\nd q| \text{lim}_{\text{lower}}\leq\nd q\leq \text{lim}_{\text{upper}}\}$

$\mathcal{C}_{\nd p}=\{\nd p| \norm{\nd x-\nd x_c}_2^2 \leq r^2\}$

\begin{equation*} \begin{array}{rll} \min_{\nd q, \nd p} & \norm{\nd q - \nd q_0}_2^2\\ \st & \nd f(\nd q) - \nd p = \nd 0, \\ & \nd q \in \mathcal{C}_{\nd q}, \\ & \nd p \in \mathcal{C}_{\nd p}, \end{array} \end{equation*}

param.radius=50. solver_reset() def fun(qp): # Cost: ||q-q0||_2^2 q = qp[:param.nbVarX] return (q-q0).dot(q-q0) def grad_fun(qp): # Gradient of the cost: 2*(q-q0) q = qp[:param.nbVarX] return np.hstack([2*(q-q0), np.zeros(2)]) def constraint_fun(qp): # Constraint is the forward kinematics: f(q)-p=0 q,p = qp[:param.nbVarX], qp[param.nbVarX:] return fkin(q, param)[:2] - p def grad_constraint_fun(qp): # Jacobian of the constraints q = qp[:param.nbVarX] J = Jkin(q, param)[:2] return np.hstack([J, -np.eye(2)]) def project(qp): # Projection function for q q_projected = np.clip(qp[:param.nbVarX], -2*np.pi, 2*np.pi) # Projection function for p x_c = param.Mu[:2] p_projected = project_quadratic(qp[param.nbVarX:]-x_c, 0, 0.5*(param.radius**2))+x_c return np.append(q_projected, p_projected) def inverse_kinematics(q): qp0 = project(np.append(q, fkin(q, param)[:2])) # initialization result = solve(qp0, fun, grad_fun, constraint_fun, grad_constraint_fun,project=project) return result.x[:param.nbVarX]

We would like the robot's end-effector to stay mode:inside/boundary/outside a rectangular region defined by its center $\nd x_c$:x_c and its size $[L_x, L_y]$:param.size ($\mathcal{C}_{\nd p}$) while respecting joint limits ($\mathcal{C}_{\nd q}$)

$\mathcal{C}_{\nd q}=\{\nd q| \text{lim}_{\text{lower}}\leq\nd q\leq \text{lim}_{\text{upper}}\}$

$\mathcal{C}_{\nd p}=\{\nd p| \norm{\nd W(\nd x-\nd x_c)}_{\infty} \leq 0.5L_x\}$

$\nd W = \text{diag}(1., L_x/L_y)$

\begin{equation*} \begin{array}{rll} \min_{\nd q, \nd p} & \norm{\nd q - \nd q_0}_2^2\\ \st & \nd f(\nd q) - \nd p = \nd 0, \\ & \nd q \in \mathcal{C}_{\nd q}, \\ & \nd p \in \mathcal{C}_{\nd p}, \end{array} \end{equation*}

param.sz = [50, 30] # Size of object mode = "boundary" # "inside", "outside" or "boundary" solver_reset() def fun(qp): # Cost: ||q-q0||_2^2 q = qp[:param.nbVarX] return (q-q0).dot(q-q0) def grad_fun(qp): # Gradient of the cost: 2*(q-q0) q = qp[:param.nbVarX] return np.hstack([2*(q-q0), np.zeros(2)]) def constraint_fun(qp): # Constraint is the forward kinematics: f(q)-p=0 q,p = qp[:param.nbVarX], qp[param.nbVarX:] return fkin(q, param)[:2] - p def grad_constraint_fun(qp): # Jacobian of the constraints q = qp[:param.nbVarX] J = Jkin(q, param)[:2] return np.hstack([J, -np.eye(2)]) def project(qp): # Projection function for q q_projected = np.clip(qp[:param.nbVarX], -2*np.pi, 2*np.pi) # Projection function for p x_c = param.Mu[:2] p_projected = project_square(qp[param.nbVarX:], center=x_c,\ width=param.sz[0], height=param.sz[1], angle=param.Mu[-1], mode=mode) return np.append(q_projected, p_projected) def inverse_kinematics(q): qp0 = project(np.append(q, fkin(q, param)[:2])) # initialization result = solve(qp0, fun, grad_fun, constraint_fun, grad_constraint_fun,project=project) return result.x[:param.nbVarX]

We would like the robot's end-effector to stay on a line defined by its slope $\nd a$:a and passing through the origin ($\mathcal{C}_{\nd p}$) while respecting joint limits ($\mathcal{C}_{\nd q}$)

$\mathcal{C}_{\nd q}=\{\nd q| \text{lim}_{\text{lower}}\leq\nd q\leq \text{lim}_{\text{upper}}\}$

$\mathcal{C}_{\nd p}=\{\nd p| \nd a^\trsp \nd x = 0\}$

\begin{equation*} \begin{array}{rll} \min_{\nd q, \nd p} & \norm{\nd q - \nd q_0}_2^2\\ \st & \nd f(\nd q) - \nd p = \nd 0, \\ & \nd q \in \mathcal{C}_{\nd q}, \\ & \nd p \in \mathcal{C}_{\nd p}, \end{array} \end{equation*}

solver_reset() def fun(qp): # Cost: ||q-q0||_2^2 q = qp[:param.nbVarX] return (q-q0).dot(q-q0) def grad_fun(qp): # Gradient of the cost: 2*(q-q0) q = qp[:param.nbVarX] return np.hstack([2*(q-q0), np.zeros(2)]) def constraint_fun(qp): # Constraint is the forward kinematics: f(q)-p=0 q,p = qp[:param.nbVarX], qp[param.nbVarX:] return fkin(q, param)[:2] - p def grad_constraint_fun(qp): # Jacobian of the constraints q = qp[:param.nbVarX] J = Jkin(q, param)[:2] return np.hstack([J, -np.eye(2)]) def project(qp): # Projection function for q q_projected = np.clip(qp[:param.nbVarX], -2*np.pi, 2*np.pi) # Projection function for p slope_angle = param.Mu[-1] p_projected = project_linear(qp[param.nbVarX:], a=np.array([np.tan(slope_angle), -1]),\ l=0, u=0) return np.append(q_projected, p_projected) def inverse_kinematics(q): qp0 = project(np.append(q, fkin(q, param)[:2])) # initialization result = solve(qp0, fun, grad_fun, constraint_fun, grad_constraint_fun,project=project) return result.x[:param.nbVarX]
solver_reset() param.orientation_noise = 1 solver_options.verbose = False param.success = np.zeros(100) param.iter = 0 def inverse_kinematics(x): param.iter += 1 param.iter = param.iter % 1000 def fun(qx): # Cost is to stay as close as possible to the initial configuration q = qx[:param.nbVarX] err = q-x return 0.5*(err).dot(err) def grad_fun(qx): # Gradient of the cost function q = qx[:param.nbVarX] err = q-x return np.hstack([err, np.zeros(2)]) def constraint_fun(qx): # Constraint is the forward kinematics q = qx[:param.nbVarX] p = qx[param.nbVarX:] return fkin(q, param)[:2] - p def grad_constraint_fun(qx): # jacobian of the constraints q = qx[:param.nbVarX] J = Jkin(q, param)[:2] return np.hstack([J, -np.eye(2)]) def project(qx): # Projection functions for q and x # q_projected = np.clip(qx[:param.nbVarX], lower_limits, upper_limits) q_projected = qx[:param.nbVarX] # x_projected = qx[param.nbVarX:] xd1 = param.Mu[:2] A = np.eye(2)*1e-2 b = np.zeros(2) c = -np.array([np.tan(param.Mu[-1]), -1])/0.84 # 0.99 probability d = 0. x_projected = project_soc_spg(qx[param.nbVarX:], A, b, c,d, max_iter=20) return np.append(q_projected, x_projected) p = fkin(x, param)[:2] qx0 = project(np.append(x, p )) # initialization param.success[param.iter]=np.array([np.tan(param.Mu[-1]), -1]).dot(p)<=0 print(["Success rate:", np.sum(param.success)]) if not param.orientation_noise: result = solve(qx0, fun, grad_fun, constraint_fun, grad_constraint_fun,project=project) return result.x[:param.nbVarX] else: return x
param.radius=5. solver_reset() solver_options.verbose=False def inverse_kinematics(x): def fun(qxM): # Cost is to stay as close as possible to the initial configuration q = qxM[:param.nbVarX] err = q-x return 0.5*(err).dot(err) def grad_fun(qxM): # Gradient of the cost function q = qxM[:param.nbVarX] err = q-x return np.hstack([err, np.zeros(2), np.zeros(4)]) def manipulability_vec(q): J = Jkin(q, param)[:2] return (J.dot(J.T)).flatten() def grad_manipulability_vec(q): e = 1E-3 X = q[:,None] + np.eye(param.nbVarX) * e F1 = manipulability_vec(q) # 4 F2 = np.stack([manipulability_vec(X[:,i]) for i in range(param.nbVarX)]).T # (4,3) J = (F2 - F1[:,None]) / e return J def constraint_fun(qxM): # Constraint is the forward kinematics q = qxM[:param.nbVarX] p = qxM[param.nbVarX:param.nbVarX+2] M = qxM[-4:] con1 = fkin(q, param)[:2] - p con2 = M - manipulability_vec(q) return np.append(con1, con2) def grad_constraint_fun(qxM): # jacobian of the constraints q = qxM[:param.nbVarX] J = Jkin(q, param)[:2] grad = np.zeros((2+4, param.nbVarX+2+4)) grad[:2, :param.nbVarX] = J # dq grad[:2, param.nbVarX:param.nbVarX+2] = -np.eye(2) # dx grad[2:, :param.nbVarX] = grad_manipulability_vec(q) #dq grad[2:, -4:] = np.eye(4) # dM return grad def project(qx): # Projection functions for q and x # q_projected = np.clip(qx[:param.nbVarX], lower_limits, upper_limits) q_projected = qx[:param.nbVarX] # x_projected = qx[param.nbVarX:] # x_projected = xd1 xd1 = param.Mu[:2] x_projected = project_quadratic(qx[param.nbVarX:param.nbVarX+2]-xd1, 0, 0.5*(param.radius**2))+xd1 return np.append(np.append(q_projected, x_projected), qx[-4:]) qx0 = project(np.append(np.append(x, fkin(x, param)[:2]), manipulability_vec(x))) # initialization result = solve(qx0, fun, grad_fun, constraint_fun, grad_constraint_fun,project=project) print(x) return result.x[:param.nbVarX]

(press shift+enter or click on the green run button to run the code; objects and joints can be moved with the mouse)

import asyncio from pyodide.ffi import create_proxy from js import Path2D from js import document import numpy as np object_angle = document.getElementById('object_angle') # Objects angle tab = document.querySelector('button[data-bs-toggle="tab"]') ######################################################################################### base1_svg = Path2D.new('m -40.741975,77.319831 c -0.47247,-4.03869 7.32825,-20.1653 10.1171,-22.57617 4.71807,-4.07862 14.00201,-4.3722 15.87822,-6.89366 1.16821,-1.06725 1.19306,-2.45846 1.19136,-4.984461 -0.005,-6.836939 0.0375,-38.9164375 -0.0588,-42.62054746 C -13.757555,-5.2728275 -9.8130348,-13.34661 -0.02248483,-13.67734 7.5903552,-13.93451 13.741895,-7.1292375 13.608255,-0.84839739 13.474625,5.4324325 13.073715,50.200081 13.741895,54.075491 c 0.66817,3.8754 3.0736,26.72695 3.0736,26.72695 l -53.47684,-0.23624 c -3.68777,-0.0163 -4.0806,-3.24637 -4.0806,-3.24637 z') base2_svg = Path2D.new('m -13.653647,45.770986 27.119789,-0.07088') seg11_svg = Path2D.new('M 1.1085815,-48.64595 C 2.8616565,-42.037584 12.141047,-7.3721658 13.181308,-3.8158258 14.730923,1.4818692 12.982058,10.29588 3.6015646,13.1191 -3.6924249,15.31437 -11.379603,10.30832 -12.856452,4.2020952 c -1.476846,-6.106188 -11.012844,-42.5297362 -12.082149,-45.6580692 -1.43181,-5.329295 -2.652606,-11.707828 -2.961653,-18.313541 -0.264086,-5.644652 2.111069,-7.347919 2.111069,-7.347919 2.624567,-3.183184 8.150604,-3.203987 10.333578,-6.275591 1.769697,-2.490098 1.823736,-5.627976 1.959877,-8.208118 0.347278,-6.581603 7.8818877,-11.888333 13.83865325,-11.31331 11.26196775,1.087146 13.17554475,9.678077 12.89920975,14.363762 -0.465778,7.897881 -5.8447437,11.223081 -10.8257944,12.5317 -4.0229212,1.0569 -4.0522977,5.558527 -3.6062254,8.077811 0.53206435,3.004955 1.69902315,6.035714 2.2984683,9.29523 z') seg12_svg = Path2D.new('m 0.05406256,-11.597507 c -6.39589386,0 -11.58398456,5.1988245 -11.58398456,11.60742169 0,6.40859681 5.1880907,11.60742231 11.58398456,11.60742231 6.39589414,0 11.58398444,-5.1988255 11.58398444,-11.60742231 0,-6.40859719 -5.1880903,-11.60742169 -11.58398444,-11.60742169 z') seg13_svg = Path2D.new('m 0.89874154,-90.983149 c -6.37570324,-0.50777 -11.96015354,4.262759 -12.46893054,10.651135 -0.508778,6.388373 4.2502031,11.982666 10.62590635,12.490434 6.37571205,0.507768 11.96015765,-4.262758 12.46893565,-10.65113 0.50878,-6.388376 -4.2501988,-11.982669 -10.62591146,-12.490439 z') seg14_svg = Path2D.new('M -24.784795,-41.659214 1.1085815,-48.64595') seg15_svg = Path2D.new('m -20.037453,-23.361462 c 0,0 0.150891,-2.736177 2.859936,-3.928038 2.698441,-1.058633 15.064238,-4.832856 18.5649072,-5.023273 3.4151981,-0.800461 4.5404475,1.903276 4.5404475,1.903276') seg21_svg = Path2D.new('m 1.0846146,-63.378335 c 0.2455591,-2.834423 3.4523451,-16.559449 4.0431711,-18.415736 1.4726648,-4.271726 5.7043363,-7.554682 6.9088533,-12.676592 0.896166,-8.180737 -5.5218419,-14.075707 -11.67006058,-13.690757 -5.14680322,0.32229 -11.25729142,3.07163 -11.71005642,12.988353 -0.245696,5.381384 2.1556935,6.934579 1.261502,10.892576 -1.067995,4.72731 -3.306673,16.43352 -4.123841,19.092346 -1.013352,3.297141 -2.321128,5.411066 -6.454795,11.635385 -4.133667,6.224321 -5.394419,14.031661 -6.200979,18.250843 -0.80656,4.219183 -2.639059,14.959257 -1.769749,20.046047 0.662189,3.874813 5.317911,7.0872532 8.194376,7.8656925 2.799342,0.6504765 3.517742,0.6405013 5.007603,2.5337107 1.489861,1.8932084 1.467073,4.13299795 2.141633,7.605938 0.4829,3.1674976 4.2207359,9.9421608 11.3304401,10.8558018 C 5.1524174,14.518915 14.875984,8.7881742 13.263942,-1.6038057 11.604726,-12.299883 3.6744317,-12.710682 0.92067775,-13.632854 -1.5420631,-15.114186 -2.6268693,-19.519275 -1.8747035,-22.72879 -1.1225409,-25.938308 1.196278,-37.889572 1.3340625,-40.676542 1.8762966,-51.644393 -0.30239687,-54.622686 1.0846146,-63.378335 Z') seg22_svg = Path2D.new('M -11.586565,0.93074939 C -11.083534,7.3068272 -5.4927791,12.069965 0.89597241,11.565935 7.284721,11.061904 12.059397,5.4810033 11.556367,-0.89507457 11.053335,-7.2711624 5.4625836,-12.034299 -0.92616504,-11.530269 -7.3149165,-11.02624 -12.089595,-5.4453385 -11.586565,0.93074939 Z') seg23_svg = Path2D.new('m -26.640574,-36.592971 c 5.304398,1.031726 26.42204728,5.61535 26.42204728,5.61535') seg24_svg = Path2D.new('m -18.97242,-7.0296766 c 0,0 5.357638,0.9161489 6.790283,-0.3224518 0,0 1.645529,-2.0773004 2.9224726,-3.1740806 1.2245317,-1.051764 3.0335173,-2.07985 3.0335173,-2.07985 1.9028326,-1.212528 2.2666634,-4.627153 3.1812597,-7.476594 1.7216337,-5.363774 1.9197573,-6.250728 1.9197573,-6.250728') seg31_svg = Path2D.new('m -28.6797,-26.841855 c -1.2675,3.57197 -1.218858,4.557009 -1.595581,8.234518 -0.376722,3.677509 -0.09415,6.442577 -0.0095,8.568278 -0.253944,2.7250156 1.116106,5.225167 1.12849,7.9985227 -0.113818,2.61245518 -0.732443,4.5287742 -1.461378,6.6813667 -0.049,4.0362406 -0.269163,8.1196006 0.283769,12.1263916 0.524743,2.889586 3.777418,3.398207 6.006756,4.487809 3.000431,1.151299 5.962802,2.459036 9.011639,3.446545 2.908512,0.626882 4.197412,-2.375507 4.231736,-4.87884 0.0854,-2.479073 0.335025,-4.760767 2.8765686,-5.44487 3.9560009,-1.216619 8.05245912,-1.946456 12.0010307,-2.99019 5.703849,-2.0129894 9.4239807,-8.5502843 7.7887937,-14.4529723 -1.270267,-5.5243102 -6.867591,-9.6714567 -12.54557065,-9.0219797 -3.01008665,0.221201 -5.63894195,1.895241 -8.24502045,3.5658663 -2.0818469,1.3351245 -1.6868669,-3.2534803 -1.7460679,-4.8326393 -0.0013,-3.276304 0.21006,-3.084655 0.0062,-4.979716 -0.203891,-1.895062 -0.264478,-4.611901 -1.494343,-8.479035 -5.412496,-0.0097 -15.221678,-0.05267 -15.221678,-0.05267 z') seg32_svg = Path2D.new('m -14.015345,-33.566241 c -1.232867,-1.390966 -2.465733,-2.781932 -3.698599,-4.172898 0.0038,-3.646334 0.02928,-7.293353 0.01923,-10.939249 -0.02501,-0.949144 -0.522837,-2.078703 -1.513796,-2.119205 -0.942425,0.01577 -1.897362,-0.08159 -2.832194,0.04493 -0.950302,0.333999 -1.133628,1.580185 -1.115778,2.522511 -0.04848,3.474219 -0.09695,6.948437 -0.145432,10.422655 -1.213181,1.407781 -2.426362,2.815561 -3.639543,4.223342 4.308705,0.006 8.617409,0.01194 12.926114,0.01791 z') seg33_svg = Path2D.new('m -12.412129,-26.867866 v -4.799995 c 0,-1.919999 -0.435344,-1.878396 -0.888867,-1.876655 -3.030562,0.01164 -14.262729,-0.07064 -14.523962,-0.04334 -0.467055,0 -0.934111,0 -0.883228,1.904343 0.01098,0.410814 0.0013,4.808677 0.0013,4.808677') seg34_svg = Path2D.new('M -10.345869,0.79321884 C -9.8879044,6.4881727 -4.8873495,10.735439 0.81892316,10.276563 6.5251942,9.8176865 10.782785,4.8259161 10.324819,-0.86903645 9.8668498,-6.5639995 4.8662942,-10.811265 -0.8399769,-10.35239 -6.5462504,-9.8935134 -10.803835,-4.901743 -10.345869,0.79321884 Z') seg35_svg = Path2D.new('m -10.926083,-10.640947 c -12.932836,-0.04585 -19.378158,-0.0931 -19.378158,-0.0931') seg36_svg = Path2D.new('M -9.9187154,15.300602 C -29.124234,15.272545 -30.475824,15.251842 -30.475824,15.251842') seg37_svg = Path2D.new('m -23.186087,-46.845579 h 5.542233 v 0') # Logarithmic map for R^2 x S^1 manifold def logmap(f, f0): diff = np.zeros(3) diff[:2] = f[:2] - f0[:2] diff[2] = np.imag(np.log(np.exp(f0[-1]*1j).conj().T * np.exp(f[-1]*1j).T)).conj() return diff # Apply angle offsets to match robot kinematic chain def emulate_DH_params(x): xt = np.copy(x) xt[0] = xt[0] - np.pi/2 orient = np.mod(np.sum(xt,0)+np.pi, 2*np.pi) - np.pi xt[2] = xt[2] - np.arctan(20.5/51) return xt, orient # Forward kinematics for end-effector (in robot coordinate system) def fkin(x, param): xt, orient = emulate_DH_params(x) L = np.tril(np.ones([param.nbVarX, param.nbVarX])) f = np.vstack([ param.l @ np.cos(L @ xt), param.l @ np.sin(L @ xt), orient ]) # f1,f2,f3, where f3 is the orientation (single Euler angle for planar robot) f[1] += 81 return f.flatten() # Forward kinematics for all joints (in robot coordinate system) def fkin0(x, param): xt, _ = emulate_DH_params(x) L = np.tril(np.ones([param.nbVarX, param.nbVarX])) f = np.vstack([ L @ np.diag(param.l) @ np.cos(L @ xt), L @ np.diag(param.l) @ np.sin(L @ xt) ]) f = np.hstack([np.zeros([2,1]), f]) f[1] += 81 return f # Jacobian with analytical computation (for single time step) def Jkin(xt, param): xt, _ = emulate_DH_params(xt) L = np.tril(np.ones([param.nbVarX, param.nbVarX])) J = np.vstack([ -np.sin(L @ xt).T @ np.diag(param.l) @ L, np.cos(L @ xt).T @ np.diag(param.l) @ L, np.ones([1,param.nbVarX]) ]) return J ## Parameters # =============================== param = lambda: None # Lazy way to define an empty class in python param.dt = 1e-1 # Time step length param.nbVarX = 3 # State space dimension (x1,x2,x3) param.nbVarF = 3 # Task space dimension (position and orientation) param.l = [79, 96, 55] # Robot links lengths param.sz = [50, 30] # Size of objects param.Mu = [90, 0, 0] # Object position and orientation param.radius = 50. param.success = np.zeros(100) param.iter = 0 param.orientation_noise = False param.tab = None solver_options = lambda: None def solver_reset(): solver_options.rho = None solver_options.lmb = None solver_options.max_iter = 1 solver_options.max_spg_iter = 10 solver_options.tol = 1e-5 solver_options.tol_rel = 1e-5 solver_options.verbose = False solver_reset() def solve(qx0, fun, grad_fun, constraint_fun, grad_constraint_fun, project=None): result = solve_AL_SPG(qx0, fun, grad_fun, constraint_fun, grad_constraint_fun, project=project, \ rho=solver_options.rho, \ lmb=solver_options.lmb, \ max_iter=solver_options.max_iter, \ max_spg_iter=solver_options.max_spg_iter, \ tol=solver_options.tol, tol_rel=solver_options.tol_rel, verbose=solver_options.verbose, subverbose=0) solver_options.rho = result.rho solver_options.lmb = result.lmb return result tabEl = document.querySelectorAll('button[data-bs-toggle="tab"]') def tab_fun(event): activated_pane = document.querySelector(event.target.getAttribute('data-bs-target')) param.tab = activated_pane.id for tab in tabEl: tab.addEventListener('shown.bs.tab', create_proxy(tab_fun)) ######################################################################################### # GUI scaling_factor = 2 # General scaling factor for rendering # Mouse events mouse0 = np.zeros(2) mouse = np.zeros(2) mousedown = 0 selected_obj = -1 hover_obj = -1 hover_joint = -1 move_joint= -1 hover0 = np.zeros(2) def onMouseMove(event): global mouse, mouse0, hover0, x offset = canvas.getBoundingClientRect() mouse0[0] = (event.clientX - offset.x) * canvas.width / canvas.clientWidth mouse0[1] = (event.clientY - offset.y) * canvas.height / canvas.clientHeight mouse[0] = (mouse0[0] - canvas.width * 0.5) / scaling_factor mouse[1] = (mouse0[1] - canvas.height * 0.5) / scaling_factor if move_joint >= 0: x[move_joint] -= 1E-2 * np.sum(hover0 - mouse0) hover0 = np.copy(mouse0) def onTouchMove(event): global mouse, mouse0, hover0, x offset = event.target.getBoundingClientRect() mouse0[0] = (event.touches.item(0).clientX - offset.x) * canvas.width / canvas.clientWidth mouse0[1] = (event.touches.item(0).clientY - offset.y) * canvas.height / canvas.clientHeight mouse[0] = (mouse0[0] - canvas.width * 0.5) / scaling_factor mouse[1] = (mouse0[1] - canvas.height * 0.5) / scaling_factor if move_joint >= 0: x[move_joint] -= 1E-2 * np.sum(hover0 - mouse0) hover0 = np.copy(mouse0) def onMouseDown(event): global mousedown, move_joint, hover0 mousedown = 1 if hover_joint >= 0: move_joint = hover_joint hover0 = np.copy(mouse0) def onMouseUp(event): global mousedown, selected_obj, move_joint mousedown = 0 selected_obj = -1 move_joint = -1 def onWheel(event): global hover_joint, hover_obj, x, object_angle if hover_obj == 0: #param.Mu[2] += 0.2 * (event.deltaY/106) object_angle.value = (float)(object_angle.value) + 0.2 * (event.deltaY/106) if hover_joint >= 0: x[hover_joint] -= 0.2 * (event.deltaY/106) document.addEventListener('mousemove', create_proxy(onMouseMove)) #for standard mouse document.addEventListener('touchmove', create_proxy(onTouchMove)) #for mobile interfaces document.addEventListener('mousedown', create_proxy(onMouseDown)) #for standard mouse #document.addEventListener('pointerdown', create_proxy(onMouseDown)) #for mobile interfaces document.addEventListener('touchstart', create_proxy(onMouseDown)) #for mobile interfaces document.addEventListener('mouseup', create_proxy(onMouseUp)) #for standard mouse #document.addEventListener('pointerup', create_proxy(onMouseUp)) #for mobile interfaces document.addEventListener('touchend', create_proxy(onMouseUp)) #for mobile interfaces document.addEventListener('wheel', create_proxy(onWheel)) #for standard mouse ######################################################################################### canvas = document.getElementById('canvas') ctx = canvas.getContext('2d') def clear_screen(): ctx.setTransform(1, 0, 0, 1, 0, 0) # Reset transformation to identity ctx.fillStyle = 'white' ctx.fillRect(0, 0, canvas.width, canvas.height) def draw_ground(): ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation ctx.beginPath() ctx.lineCap = 'round' ctx.lineJoin = 'round' ctx.lineWidth = '5' ctx.strokeStyle = '#CCCCCC' ctx.moveTo(-200, 164) ctx.lineTo(200, 164) ctx.stroke() def draw_robot(xt, color1, color2, color3, color4, selectable): global hover_joint ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation # Draw base ctx.translate(0, 81) ctx.lineWidth = '1' ctx.strokeStyle = color3 ctx.fillStyle = color1 ctx.fill(base1_svg) ctx.stroke(base1_svg) # Outline ctx.stroke(base2_svg) # Draw seg1 ctx.rotate(xt[0]) ctx.fillStyle = color1 ctx.fill(seg11_svg) ctx.stroke(seg11_svg) # Outline ctx.fillStyle = color2 if selectable and ctx.isPointInPath(seg12_svg, mouse0[0], mouse0[1]): ctx.fillStyle = '#3399FF' hover_joint = 0 ctx.fill(seg12_svg) ctx.stroke(seg12_svg) ctx.stroke(seg13_svg) ctx.stroke(seg14_svg) ctx.stroke(seg15_svg) # Draw seg2 ctx.translate(0, -79) ctx.rotate(xt[1]) ctx.fillStyle = color1 ctx.fill(seg21_svg) ctx.stroke(seg21_svg) # Outline ctx.fillStyle = color2 if selectable and ctx.isPointInPath(seg22_svg, mouse0[0], mouse0[1]): ctx.fillStyle = '#FF9933' hover_joint = 1 ctx.fill(seg22_svg) ctx.stroke(seg22_svg) ctx.stroke(seg23_svg) ctx.stroke(seg24_svg) # Draw seg3 ctx.translate(0, -96) ctx.rotate(xt[2]) ctx.fillStyle = color1 ctx.fill(seg31_svg) ctx.stroke(seg31_svg) # Outline ctx.fill(seg32_svg) ctx.stroke(seg32_svg) # Outline ctx.fill(seg33_svg) ctx.stroke(seg33_svg) # Outline ctx.fillStyle = color2 if selectable and ctx.isPointInPath(seg34_svg, mouse0[0], mouse0[1]): ctx.fillStyle = '#99FF33' hover_joint = 2 ctx.fill(seg34_svg) ctx.stroke(seg34_svg) ctx.stroke(seg35_svg) ctx.stroke(seg36_svg) ctx.stroke(seg37_svg) # Draw end-effector point ctx.translate(-20.5, -51) ctx.beginPath() ctx.arc(0, 0, 2, 0, 2 * np.pi) ctx.fillStyle = color4 ctx.fill() # # Draw skeleton of the kinematic chain # ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation # ctx.lineCap = 'round' # ctx.lineJoin = 'round' # ctx.lineWidth = '2' # ctx.strokeStyle = '#FF8888' # f = fkin0(x, param) # ctx.beginPath() # ctx.moveTo(0, 81) # for i in range(param.nbVarX+1): # ctx.lineTo(f[0,i], f[1,i]) # ctx.stroke() def draw_obj(param, color, colortxt): global selected_obj, hover_obj ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation ctx.translate(param.Mu[0], param.Mu[1]) ctx.rotate(param.Mu[2]) # Draw object ctx.fillStyle = color obj = Path2D.new() obj.rect(-param.sz[0]/2, -param.sz[1]/2, param.sz[0], param.sz[1]) ctx.fill(obj) if ctx.isPointInPath(obj, mouse0[0], mouse0[1]): hover_obj = 0 if ctx.isPointInPath(obj, mouse0[0], mouse0[1]) and mousedown==1: selected_obj = 0 #ctx.fillRect(-param.sz[0]/2, -param.sz[1]/2, param.sz[0], param.sz[1]) if param.sz[0] > 39 and param.sz[1] > 19: ctx.textAlign = 'center' ctx.textBaseline = 'middle' ctx.font = '10px Permanent Marker' ctx.fillStyle = colortxt #ctx.fillText('Move me!', 0, 0) def draw_obj_circle(param, color, colortxt): global selected_obj, hover_obj ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation ctx.translate(param.Mu[0], param.Mu[1]) ctx.rotate(param.Mu[2]) # Draw object ctx.fillStyle = color obj = Path2D.new() obj.arc(0,0,param.radius,0,2*np.pi) #obj.rect(-param.sz[0]/2, -param.sz[1]/2, param.sz[0], param.sz[1]) ctx.fill(obj) if ctx.isPointInPath(obj, mouse0[0], mouse0[1]): hover_obj = 0 if ctx.isPointInPath(obj, mouse0[0], mouse0[1]) and mousedown==1: selected_obj = 0 #ctx.fillRect(-param.sz[0]/2, -param.sz[1]/2, param.sz[0], param.sz[1]) if param.sz[0] > 39 and param.sz[1] > 19: ctx.textAlign = 'center' ctx.textBaseline = 'middle' ctx.font = '10px Permanent Marker' ctx.fillStyle = colortxt #ctx.fillText('Move me!', 0, 0) def draw_line(param, color, colortxt): global selected_obj, hover_obj ctx.setTransform(scaling_factor, 0, 0, scaling_factor, canvas.width*0.5, canvas.height*0.5) # Reset transformation ctx.beginPath() ctx.lineCap = 'round' ctx.lineJoin = 'round' ctx.lineWidth = '5' ctx.strokeStyle = color ctx.moveTo(-1000, -1000*np.tan(param.Mu[-1])) ctx.lineTo(1000, 1000*np.tan(param.Mu[-1])) ctx.stroke() ######################################################################################### def errorHandler(e): msg = 'Error: ' + str(e) #console.error(msg) el = document.getElementById('repl-err') el.innerText = msg #el.textContent = msg def inverse_kinematics(x): return x ######################################################################################### x = np.array([-np.pi/4, np.pi/2, np.pi/4]) # Initial robot state q0 = x.copy() u = np.zeros(param.nbVarX) count_reset = 0 async def main(): global inverse_kinematics, hover_joint, hover_obj,x, param while True: try: x = inverse_kinematics(x) except Exception as e: errorHandler(e) def inverse_kinematics(x): return x if solver_options.rho is not None: if solver_options.rho > 1e13: solver_reset() if (selected_obj >= 0) : if count_reset == 0: solver_reset() count_reset = 1 else: count_reset = 0 # Reinit hovering variables hover_joint = -1 hover_obj = -1 f = fkin(x, param) # Rendering clear_screen() draw_ground() if param.tab == 'IK-tab-pane': draw_obj_circle(param, '#FF3399', '#DD1177') elif param.tab == 'dIK-tab-pane': draw_obj(param, '#FF3399', '#DD1177') elif param.tab == 'wIK-tab-pane' or param.tab == 'pIK-tab-pane': draw_line(param, '#FF3399', '#DD1177') else: draw_obj_circle(param, '#FF3399', '#DD1177') #q0 = x.copy() draw_robot(q0, '#EEEEEE', '#DDDDDD', '#CCCCCC', '#BBBBBB', False) # x0 draw_robot(x, '#CCCCCC', '#AAAAAA', '#222222', '#000000', True) # Object selection if selected_obj==0: param.Mu[:2] = mouse param.Mu[0] = max(min(param.Mu[0],225), -225) param.Mu[1] = max(min(param.Mu[1],175), -175) param.Mu[2] = (float)(object_angle.value) + np.random.normal(scale=1e-2)*param.orientation_noise await asyncio.sleep(0.0001) #asyncio.ensure_future(main()) pyscript.run_until_complete(main()) nb_max_iterations_line_search = 40 alphas = 10.**np.linspace(0., -5., nb_max_iterations_line_search) def compute_gamma(s, y, gamma, gamma_min=1e-20, gamma_max=1e2, verbose=False): sTy = s.dot(y) if sTy > 0: # mu = 1e-6 # spectral_param1 = s.dot(s) / sTy # spectral_param2 = sTy/ y.dot(y) # tmp = (s*y+ mu*gamma)/(s*s + mu) # gamma = 1/tmp # gamma[tmp < 1/spectral_param1] = spectral_param1 # gamma[tmp > 1/spectral_param2] = spectral_param2 spectral_param1 = s.dot(s) / sTy spectral_param2 = sTy / y.dot(y) if spectral_param1 < 2*spectral_param2: spectral_param = spectral_param2 else: spectral_param = spectral_param1-0.5*spectral_param2 gamma = np.clip(spectral_param, gamma_min, gamma_max) else: if verbose: print("PSD problem.") gamma = gamma*10. # gamma = 1. return gamma def compute_gamma_init(dfk, xk=None, project=None, gamma_min=1e-20, gamma_max=1e2): if project is None: param = np.linalg.norm(dfk, ord=np.inf) else: param = np.linalg.norm(project(xk-dfk)-xk, ord=np.inf) if param >= 1e-5: gamma = np.clip(1./param, gamma_min, gamma_max) else: gamma = gamma_max return gamma def solve_SPG(x0, fun, grad_fun, project=None, m=5, gamma=None,max_iter=100, tol=1e-5, tol_rel=1e-8, keep_gamma_constant=False, keep_log_iterations=False, verbose=0): """ Solves min_x fun(x) for x \in R^x_dim without constraints but with projections. Parameters ---------- x0 : ndarray Initial guess ( R^x ) fun : Function Objective function ( R^x --> R) grad_fun : Function Gradient of the objective function ( R^x --> R^x) project : Function Projection function (R^x --> R^x) m : int Number of previous iterations to check for nonmonotone line search gamma : float (default=None) Initial spectral step size, if None, computed with an heuristic. max_iter : int Maximum number of iterations tol : float Tolerance threshold for the constraint function keep_gamma_constant : bool (default=False) Keep gamma=1 to reduce it to nonmonotone line search steepest gradient descent. keep_log_iterations : bool (default=False) Keeps a log of the decision variable at each iteration until convergence. verbose : bool True if you want to see results Returns ------- result object with the following attributes result.x : Final solution result.fun : Final objective cost result.gamma : Final gamma result.nb_iter : Number of iterations achieved result.x_log : Log of the decision variable """ max_line_search_iter = 20 xk = x0.copy() if keep_log_iterations: x_log = [xk] fk = fun(xk) dfk = grad_fun(xk) # Choose lambda and gamma, this strategy seems to be the best if gamma is None: gamma = compute_gamma_init(dfk, xk, project, gamma_min=1e-3, gamma_max=1e3) gamma_dfk = gamma*dfk if verbose: print("Iteration {:>3}: | Objective: ".format(0) + f"{fk:.3e}" + " | Gamma: " + f"{gamma:.2e}") fk_log = [fk] for i in range(max_iter): if project is not None: x0_ = xk - gamma_dfk if not type(project) == list: dk = project(x0_) - xk else: xk_ = project_dykstra(project, x0_, max_iter=100) dk = xk_ - xk else: dk = - gamma_dfk # Check convergence: if the norm of the search direction is small, it converged if np.linalg.norm(dfk) < tol and i!=0: if verbose: print("[Search direction] Converged at iteration", i, "with an objective value of", "{:2e}".format(fk)) break # Line search f_max = np.max(fk_log[-m:]) xk_tentative = xk + dk fk_tentative = fun(xk_tentative) tmp = dfk.dot(dk) alpha = 1. count_ls_iter = 0 ls_success = True while fk_tentative > f_max + 1e-4*alpha*tmp: alpha_tmp = -0.5*(alpha**2)*tmp/(fk_tentative - fk - alpha*tmp) if alpha_tmp >= 0.1 and alpha_tmp <= 0.9*alpha: alpha = alpha_tmp else: alpha = 0.5*alpha xk_tentative = xk + alpha*dk fk_tentative = fun(xk_tentative) count_ls_iter += 1 if count_ls_iter > max_line_search_iter: ls_success = False break if not ls_success: if verbose: print("Line search could not find a better solution") if i == 0: gamma = gamma*10 gamma_dfk = gamma*dfk continue else: break # print("h") # # Line search # ls_success = False # tmp = dfk.dot(dk) # f_max = np.max(fk_log[-m:]) # for alpha in alphas: # xk_tentative = xk + alpha * dk # fk_tentative = fun(xk_tentative) # if fk_tentative <= f_max + 1e-4 * alpha * tmp: # ls_success = True # print_success = "Success" # break # # Check if line search failed # if not ls_success: # # return xk, xk # print_success = "Fail" # if verbose: print(print_success) # gamma = gamma*10 # gamma_dfk = gamma*dfk # continue if np.abs(fk_tentative - fk) < tol_rel: if verbose: print("[Relative objective reduction] Converged at iteration", i, "with an objective value of", "{:2e}".format(fk)) break fk_log += [fk] sk = xk_tentative - xk xk = xk_tentative fk = fk_tentative if keep_log_iterations: x_log += [xk] yk = grad_fun(xk) - dfk dfk = yk + dfk gamma = compute_gamma(sk, yk, gamma, verbose=verbose) if keep_gamma_constant: gamma = 1. gamma_dfk = gamma * dfk if verbose: print("Iteration {:>3}: | Objective: ".format( i+1) + f"{fk:.3e}" +" | Gamma: " + f"{np.max(gamma):.2e}", alpha) result = lambda: None result.x = xk result.fun = fk result.gamma = gamma result.nb_iter = i if keep_log_iterations: result.log = np.stack(x_log) return result def project_quadratic(x,l,u): """ Projects the vector x such that l<= 0.5 * (x.T @ x) <= u """ if x.ndim == 2: return project_quadratic_batch(x, l, u) else: # raise NotImplementedError return project_quadratic_batch(x[None], l, u)[0] # val = 0.5 * (x.T.dot(x)) # # if val > u: # z = x * np.sqrt(2 * u)/ np.linalg.norm(x) # zs = [z, -z] # return zs[np.argmin([np.linalg.norm(z-x), np.linalg.norm(-z-x)])] # elif l > val: # z = x * np.sqrt(2 * l) / np.linalg.norm(x) # zs = [z, -z] # return zs[np.argmin([np.linalg.norm(z - x), np.linalg.norm(-z - x)])] # else: # return x def project_quadratic_batch(x,l,u): """ Batch version of project_quadratic, x is a matrix """ z = x.copy() xTx = np.sum(x * x, axis=-1) val = 0.5 * xTx x_norm = np.sqrt(xTx) cond1 = np.where(np.logical_and(val > u, x_norm > 1e-5)) cond2 = np.where(np.logical_and(l > val, x_norm > 1e-5)) cond3 = np.where(x_norm <= 1e-5) z[cond1] = x[cond1] * np.sqrt(2 * u)/ x_norm[cond1][:,None] z[cond2] = x[cond2] * np.sqrt(2 * l)/ x_norm[cond2][:,None] if len(cond3) > 0: vec = np.random.randn(len(cond3), x.shape[-1]) vec = vec / np.linalg.norm(vec, axis=-1)[:,None] z[cond3] = vec*np.sqrt(2*l) return z def project_square(x, center, width, height, angle=0., mode="boundary"): """ Projects the vector x such that l <= ||x||_{\infty} <= u This defines the region defined by between two square regions centered at center. mode={"inside", "outside", "boundary"} """ xc = x-center R = np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]]) xy = np.array([width,height]) xy_ = width/xy W = np.diag(xy_) if mode == "boundary": lower = width/2 upper = width/2 elif mode == "inside": lower = 0. upper = width/2 elif mode == "outside": lower = width/2 upper = 1e5 W_ = W@R.T W_inv = np.linalg.inv(W_) xc = W_ @ xc j = np.argmax(np.abs(xc)) if np.abs(xc[j]) < lower: xc[j] = lower*np.sign(xc[j]) return W_inv@np.maximum(np.minimum(xc, upper), -upper) + center def project_square_c(x, c, l, u): """ Projects the vector x such that l <= ||x-c||_{\infty} <= u This defines the region defined by between two square regions centered at c. """ z = x - c z_ = project_square(z, l, u) return z_ + c def project_linear(x, a, l, u): """ Projects the vector x such that l<= a.T @ x <= u """ aTx = a.dot(x) aTa = a.dot(a) + 1e-30 if aTx > u: mu = aTx - u elif aTx < l: mu = aTx - l else: mu = 0. return x - mu*a/aTa def project_soc_unit(zt): """ Projects the vector z and t onto the second order cone (soc) such that ||z||<= t """ z = zt[...,:-1] t = zt[...,-1] if z.ndim == 2: z_, t_ = project_soc_unit_batch(z=z, t=t) return np.concatenate([z_, t_[:, None]], axis=1) else: z_norm = np.linalg.norm(z) if z_norm <= t: z_, t_ = z,t elif z_norm <= -t: z_, t_ = z*0, t*0 else: tmp = (z_norm + t)/2 z_, t_ = tmp*z/(z_norm+1e-30), tmp return np.append(z_, t_) def project_soc_spg(x0, A, b, c, d, max_iter=100, tol=1e-7, verbose=False): """ Projects x0 such that ||Ax-b|| <= cTx + d """ x_dim = x0.shape[0] z_dim = A.shape[0] # also dim of b r_side = np.append(b,d) l_side = np.vstack([A, c[None]]) fun = lambda xzt: 0.5*(xzt[:x_dim]-x0).dot(xzt[:x_dim]-x0) grad_fun = lambda xzt: np.append(xzt[:x_dim]-x0, np.zeros(z_dim+1)) def project_fun1(xzt): # Projection on second order cone return np.append(xzt[:x_dim], project_soc_unit(xzt[x_dim:])) def constraint_fun(xzt): x = xzt[:x_dim] zt = xzt[x_dim:] return zt - l_side.dot(x) - r_side def grad_constraint_fun(xzt): grad = np.zeros((z_dim+1, x_dim+z_dim+1)) grad[:, :x_dim] = -l_side grad[:, x_dim:] = np.eye(z_dim+1) return grad xzt0 = np.append(np.append(x0, A@x0 + b), c.dot(x0) + d) xzt0 = project_fun1(xzt0) return solve_AL_SPG(xzt0, fun, grad_fun, constraint_fun, grad_constraint_fun, rho=1e0, project=project_fun1,tol=1e-6,tol_rel=tol, max_iter=max_iter, max_spg_iter=5, m=5, verbose=verbose).x[:x_dim] def solve_AL_SPG(x0, fun, grad_fun, constraint_fun=None, grad_constraint_fun=None, project=None, m=5, lmb=None, rho=None, max_spg_iter=10, keep_log_iterations=False, max_iter=100, tol=1e-3, tol_rel=1e-6, verbose=0, subverbose=0): """ Solves min_q fun(q) s.t. constraint_fun(q) = 0 using augmented lagrangian combined with SPG. Parameters ---------- x0 : initial point fun : objective function ( R^q -> R) grad_fun : gradient of the objective function ( R^q -> R^q) constraint_fun : scalar constraint function ( R^q -> R) grad_constraint_fun : gradient of the scalar constraint function ( R^q -> R^q) gamma : (optional, default=1.0) Initial spectral gradient step size m : (optional, default=10) number of previous iterations to take into account in the nonmonotone line search max_iter: maximum number of iterations tol : tolerance threshold for the constraint function verbose : True if you want to see results Returns ------- q_opt : result """ xk = x0.copy() objective_value = fun(xk) constraint_value = constraint_fun(xk) lmb = np.zeros_like(constraint_value) rho = np.clip(2*np.abs(objective_value)/constraint_value.dot(constraint_value), 1e-6, 10.) if rho is None else rho if np.isnan(rho): rho = 10 cons_norm = np.linalg.norm(constraint_value, ord=np.inf) gamma = None if keep_log_iterations: x_log = [xk] if verbose: print("Iteration {:>3}: | Objective: ".format( 0) + f"{objective_value:.3e}" + " | Max constraint: " + f"{np.max(constraint_value):.3e}" + " | Max lmb: " + f"{np.max(lmb):.2e}") for i in range(max_iter): def lagrangian(x): cons = constraint_fun(x) return fun(x) + lmb.dot(cons) + 0.5 * rho * cons.dot(cons) def grad_lagrangian(x): grad_cons = grad_constraint_fun(x) cons = constraint_fun(x) return grad_fun(x) + grad_cons.T.dot(lmb) + rho * grad_cons.T.dot(cons) res = solve_SPG(xk, lagrangian, grad_lagrangian, m=m, project=project, max_iter=max_spg_iter , tol=1e-6, tol_rel=1e-8, verbose=subverbose) xk = res.x if keep_log_iterations: x_log += [xk] objective_value_prev = np.copy(objective_value) if i != 0 else 1e5 objective_value = fun(xk) constraint_value = constraint_fun(xk) lagrangian_value = objective_value + lmb.dot(constraint_value) + 0.5*rho*constraint_value.dot(constraint_value) prev_cons_norm = np.copy(cons_norm) cons_norm = np.linalg.norm(constraint_value, ord=np.inf) if cons_norm< tol and np.abs(objective_value - objective_value_prev) < tol_rel: if verbose: print("Converged at iteration {:>3}: | Objective: ".format( i+1) + f"{objective_value:.3e}" + " | Max constraint: " + f"{np.max(constraint_value):.3e}" + " | Max lmb: " + f"{np.max(lmb):.2e}" + " | Lagrangian: " + f"{lagrangian_value:.2e}" ) break lmb += rho * constraint_value lmb = np.clip(lmb,-1e10, 1e10) if cons_norm > 0.1*prev_cons_norm: rho = np.clip(5 * rho, -1, 1e15) if verbose: print("Iteration {:>3}: | Objective: ".format( i+1) + f"{objective_value:.3e}" + " | Max constraint: " + f"{np.max(constraint_value):.3e}" + " | Max lmb: " + f"{np.max(lmb):.2e}" + " | Lagrangian: " + f"{lagrangian_value:.2e}" + " | Rho: " + f"{rho:.2e}" ) result = lambda: None result.x = xk result.fun = objective_value result.constraint_fun = constraint_value result.gamma = res.gamma result.rho = rho result.lmb = lmb result.nb_iter = i if keep_log_iterations: result.log = np.stack(x_log) return result