{ "cells": [ { "cell_type": "code", "execution_count": 10, "metadata": { "ExecuteTime": { "end_time": "2020-10-25T19:56:25.689185Z", "start_time": "2020-10-25T19:56:25.589964Z" } }, "outputs": [ { "data": { "text/plain": [ "array([[0.6023982 ],\n", " [0. ],\n", " [0.43053813]])" ] }, "execution_count": 10, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# by Saeed Shaker\n", "\n", "# This is a direct translation of Yuval Tassa's Matlab code into Python:\n", "# https://benjaminmoll.com/wp-content/uploads/2020/06/LCP.m\n", "# It solves LCP using a Newton type method\n", "# To be consistent across platforms and with Yuval Tassa's code,\n", "# I have tried to make as minimal changes as I could, \n", "# so this code can be followed the same way as the original Matlab code does.\n", "\n", "import numpy as np\n", "import scipy.sparse as sparse\n", "from scipy.sparse.linalg import spsolve\n", "\n", "def LCP_python(M,q,l=[],u=[],x0=[],display=False): \n", " tol = 1.0e-8;\n", " mu = 1e-3;\n", " mu_step = 5;\n", " mu_min = 1e-5;\n", " max_iter = 25;\n", " b_tol = 1e-6;\n", "\n", " n = M.shape[0]\n", "\n", " if l == []:\n", " l = np.zeros((n,1))\n", " if u == []:\n", " u = np.ones((n,1))*np.inf\n", " if x0 == []:\n", " x00 = np.maximum(np.ones((n,1)),l)\n", " x0 = np.minimum(x00,u)\n", " \n", " M = sparse.csc_matrix(M)\n", " q = q.reshape((-1, 1))\n", " l = l.reshape((-1, 1))\n", " u = u.reshape((-1, 1))\n", " x0 = x0.reshape((-1, 1))\n", " \n", " lu = np.column_stack((l , u));\n", " x = x0.copy();\n", " psi,phi,J = FB(x,q,M,l,u);\n", " new_x = True\n", "\n", " for iter1 in range(0,max_iter):\n", " if new_x:\n", " mlu = np.min(np.column_stack((np.abs(x-l),np.abs(u-x))),1).reshape((-1, 1));\n", " ilu = np.argmin(np.column_stack((np.abs(x-l),np.abs(u-x))),1).reshape((-1, 1));\n", " bad = np.maximum(np.abs(phi),mlu) < b_tol;\n", " psi = psi - 0.5*np.dot(phi[bad] , phi[bad])\n", " notbad = bad == False\n", " Jind = np.dot(notbad , notbad.T)\n", " notbad_trues = np.sum(notbad*1)\n", " J = sparse.csc_matrix(np.reshape(J[Jind] , (notbad_trues,notbad_trues) ))\n", " phi = phi[notbad]; \n", " new_x = False;\n", " nx = x.copy();\n", " nx[bad] = lu.flatten()[(bad[bad])*1+(ilu[bad]-1)*n]\n", " H = np.dot(J.T , J) + mu*sparse.eye(notbad_trues);\n", " Jphi = sparse.csc_matrix.dot(J.T,phi)\n", " d = -spsolve(sparse.csc_matrix(H) , Jphi)\n", " nx[notbad] = x[notbad] + d;\n", " npsi,nphi,nJ = FB(nx,q,M,l,u);\n", " \n", " r = (psi - npsi)/ -(np.dot(Jphi.T,d) + 0.5*np.dot(sparse.csc_matrix.dot(d.T,H),d) ); # actual reduction / expected reduction\n", "\n", " if r < 0.3:\n", " mu = np.maximum(mu*mu_step,mu_min);\n", " if r > 0:\n", " x = nx.copy();\n", " psi = npsi.copy();\n", " phi = nphi.copy();\n", " J = nJ.copy();\n", " new_x = True;\n", " if r > 0.8: \n", " mu = mu/mu_step * (mu > mu_min);\n", " if display:\n", " print('iter = ', iter1 , ' --- psi = ' , psi ,' --- r = ' , r ,' --- mu = ' , mu);\n", " if psi < tol:\n", " break;\n", " x = np.minimum(np.maximum(x,l),u); \n", " return x\n", "\n", "#----------------------------------------------------------\n", "\n", "def FB(x,q,M,l,u):\n", " n = x.size;\n", " Zl = ((l >-np.inf) & (u==np.inf))\n", " Zu = (l==-np.inf) & (u -np.inf) & (u