importosimportnumpyasnpfromscipyimportlinalgimportmatplotlib.pyplotaspltfromdatetimeimportdatetimeimporttimefromremoveredundantconimportremoveredundantconfromfindinteriorpointimportfindinteriorpointfromhalfspace2verteximporthalfspace2vertex# Module-level log file (persistent state across calls)._logfile=[None]deflog_(msg=None,*args):"""Write to log. Pass a file object to open it; call with no args to close."""ifhasattr(msg,'write'):_logfile[0]=msgelifmsgisNone:if_logfile[0]isnotNone:_logfile[0].close()_logfile[0]=Noneelif_logfile[0]isnotNone:_logfile[0].write(msg%argsifargselsemsg)_logfile[0].flush()defviridiscolors(n):"""Returns n colors from the viridis colormap as an (n, 3) RGB array."""cmap=plt.cm.get_cmap('viridis',n)returnnp.array([cmap(i)[:3]foriinrange(n)])defbin2hex(bits):"""Converts a boolean vector to a hexadecimal string."""hexchars='0123456789ABCDEF'bits=np.asarray(bits,dtype=float).flatten()Npad=(-len(bits))%4bits=np.concatenate([np.zeros(Npad),bits])Nhex=len(bits)//4digits=np.array([8,4,2,1])@bits.reshape(4,Nhex)return''.join(hexchars[int(d)]fordindigits)defgetcrid(active):"""Returns the string ID of the given set of active constraints."""return'CR_'+bin2hex(active)defscalerows(A,b,zerotol=1e-12):"""Scales rows of [A, b] to unit norm; zeros trivial rows."""b=np.asarray(b,dtype=float).flatten()Z=np.hstack([A,b[:,None]])scale=np.sqrt(np.sum(Z**2,axis=1))trivrows=(scale==0)scale[trivrows]=1.0A=A/scale[:,None]b=b/scaleb[trivrows]=1.0A[np.abs(A)<zerotol]=0.0returnA,b,scaledefcheckcrfullrank(dmpqp,activecon):"""Checks whether the active-constraint submatrix of H is positive definite."""i=activeconHii=dmpqp['H'][np.ix_(np.where(i)[0],np.where(i)[0])]ifHii.size==0:returnTrue,Hiitry:Hiihalf=linalg.cholesky(Hii,lower=False)# upper triangularfullrank=bool(np.all(np.diag(Hiihalf)>1e-6))exceptlinalg.LinAlgError:Hiihalf=np.zeros_like(Hii)fullrank=Falsereturnfullrank,Hiihalfdefgetcriticalregion(pmpqp,dmpqp,crset):"""Returns the critical region and optimal control law for a given active set."""activecon=crset['active']if'Hiihalf'incrset:Hiihalf=crset['Hiihalf']okay=Trueelse:okay,Hiihalf=checkcrfullrank(dmpqp,activecon)cr={}ifnotokay:returncr,Falsei=activeconj=~activeconi_idx=np.where(i)[0]j_idx=np.where(j)[0]# Solve for dual variable as affine function of parameters:# Y * [p; 1] = lambda*(p), using Hiihalf (upper Cholesky of H[i,i]).RHS=np.column_stack([dmpqp['D'][i_idx,:],dmpqp['d'][i_idx]])Y=-linalg.cho_solve((Hiihalf,False),RHS)# (Ni, Np+1)# Complementary slackness for inactive constraints.W=dmpqp['H'][np.ix_(j_idx,i_idx)]@Y+ \
np.column_stack([dmpqp['D'][j_idx,:],dmpqp['d'][j_idx]])# (Nj, Np+1)P=np.vstack([W,Y])# (Nj+Ni, Np+1)A=-P[:,:-1]# critical region: A*p <= bb=P[:,-1]# Screen out near-zero rows.Anorms=np.max(np.abs(A),axis=1)badrows=(Anorms<1e-10)ifnp.any(b[badrows]<0):returncr,FalseA[badrows,:]=0.0b[badrows]=1.0# Append parameter bounding region.Ne=len(pmpqp['e'])A=np.vstack([A,pmpqp['E']])b=np.concatenate([b,pmpqp['e']])A,b,_=scalerows(A,b)x,okay,_,_=findinteriorpoint(A,b)ifnotokay:returncr,Falsecr['A']=Acr['b']=bcr['cind']=np.concatenate([j_idx,i_idx,-np.arange(1,Ne+1)]).astype(int)cr['ctype']=np.array(['w']*len(j_idx)+['y']*len(i_idx)+['e']*Ne)cr['x']=np.asarray(x).flatten()# Optimal control law: z*(p) = Z*p + z0.G_i=pmpqp['G'][i_idx,:]inner=G_i.T@Y+np.column_stack([pmpqp['F'],pmpqp['c']])# (Nu, Np+1)law=-linalg.cho_solve((pmpqp['Qhalf'],False),inner)# (Nu, Np+1)cr['Z']=law[:,:-1]cr['z0']=law[:,-1]returncr,Truedefsolveprimalqp(x,pmpqp):"""Solves the primal QP at a given parameter value x."""fromscipy.optimizeimportminimizeQ=pmpqp['Q']f=(pmpqp['F']@np.asarray(x).flatten()).flatten()A_ineq=pmpqp['G']b_ineq=(pmpqp['S']@np.asarray(x).flatten()+pmpqp['W']).flatten()n=Q.shape[0]result=minimize(fun=lambdaz:0.5*z@Q@z+f@z,jac=lambdaz:Q@z+f,x0=np.zeros(n),method='SLSQP',constraints={'type':'ineq','fun':lambdaz:b_ineq-A_ineq@z,'jac':lambdaz:-A_ineq},options={'ftol':1e-10,'maxiter':500,'disp':False},)z=result.xifresult.successelsenp.full(n,np.nan)feas=result.successslack=A_ineq@z-b_ineqactive=np.abs(slack)<1e-5returnz,feas,slack,activedefaddcr(opensets,nopen,newcr):"""Adds a new candidate critical region to the open set."""nopen+=1opensets[f'N{nopen}']=newcrlog_(f" Adding {newcr['id']}\n")returnopensets,nopendeffindadjacentcr(cr,pmpqp):"""Finds an adjacent CR by stepping across the given facet and solving the QP."""ineq=np.ones(cr['A'].shape[0],dtype=bool)ineq[cr['facet']]=FalseAeq=cr['A'][[cr['facet']],:]beq=cr['b'][[cr['facet']]]x0,okay,_,_=findinteriorpoint(cr['A'][ineq,:],cr['b'][ineq],Aeq,beq)ifokay:normal=cr['A'][cr['facet'],:]x0p=np.asarray(x0).flatten()+1e-4*normal/np.linalg.norm(normal)_,feas,_,newactive=solveprimalqp(x0p,pmpqp)else:feas=Falsenewactive=np.array([],dtype=bool)ifnotokayornotfeas:newactive=np.array([],dtype=bool)returnnewactive,feasdefsolvempqp(prob,**kwargs):""" Solves the multiparametric QP: min 0.5*z'*Q*z + p'*C'*z z s.t. A*z <= b + S*p for all p satisfying E*p <= e. prob must contain Q, C, A, b, S. E and e are optional. """defaults={'display':True,'logfile':'','vertices':np.nan,'plot':False,'maxiter':np.inf,'ascell':True,'maxtime':np.inf,'printevery':100,}options={**defaults,**kwargs}ifoptions['logfile']:log_(open(options['logfile'],'w'))log_(f"Log started {datetime.now()}\n")# Convert to Bemporad (2015) nomenclature.pmpqp={}pmpqp['G']=np.asarray(prob['A'])pmpqp['W']=np.asarray(prob['b']).flatten()pmpqp['S']=np.asarray(prob['S'])pmpqp['Q']=np.asarray(prob['Q'])pmpqp['Q']=0.5*(pmpqp['Q']+pmpqp['Q'].T)pmpqp['F']=np.asarray(prob['C'])pmpqp['c']=np.zeros(pmpqp['F'].shape[0])pmpqp['Qhalf']=linalg.cholesky(pmpqp['Q'],lower=False)# upper triangularpmpqp['E']=np.asarray(prob.get('E',np.zeros((0,pmpqp['S'].shape[1]))))ifpmpqp['E'].size==0:pmpqp['e']=np.zeros(0)else:if'e'notinprob:raiseValueError('prob["e"] must be provided if prob["E"] is given!')pmpqp['e']=np.asarray(prob['e']).flatten()ifnp.isnan(options['vertices']):options['vertices']=(pmpqp['S'].shape[1]==2)# Dual problem matrices.Qinv_G=linalg.cho_solve((pmpqp['Qhalf'],False),pmpqp['G'].T)# Q^{-1} G'dmpqp={}dmpqp['H']=pmpqp['G']@Qinv_Gdmpqp['D']=pmpqp['G']@linalg.cho_solve((pmpqp['Qhalf'],False),pmpqp['F'])+pmpqp['S']dmpqp['d']=pmpqp['G']@linalg.cho_solve((pmpqp['Qhalf'],False),pmpqp['c'])+pmpqp['W']# qhull options (passed as dict to ConvexHull via removeredundantcon).qhullstr='Qt QbB Pp'ifpmpqp['S'].shape[1]>=5:qhullstr+=' Qx'qhullopts={'qhull_options':qhullstr}# Initialise: no constraints active at the origin.Ncon=dmpqp['D'].shape[0]init_active=np.zeros(Ncon,dtype=bool)opensets={'N1':{'active':init_active,'id':getcrid(init_active)}}nopen=1nfound=0regions={}probedregions={opensets['N1']['id']:[]}niter=0starttime=time.time()maxtime=options['maxtime']+starttimewhilenopen>0andniter<options['maxiter']andtime.time()<maxtime:niter+=1ifoptions['display']andniter%options['printevery']==0:print(f"Iteration {niter} ({nfound} found, {nopen} left to search).")log_(f"Iteration {niter}\n")activestr=f'N{nopen}'nopen-=1currentset=opensets.pop(activestr)crid=currentset['id']log_(f" Getting {crid}\n")cr,okay=getcriticalregion(pmpqp,dmpqp,currentset)ifokay:log_(" Found representation\n")nonredundant,cr['A'],cr['b'],_,_=removeredundantcon(cr['A'],cr['b'],cr['x'],None,qhullopts)cr['cind']=cr['cind'][nonredundant]cr['ctype']=cr['ctype'][nonredundant]ifoptions['vertices']:cr['V'],_=halfspace2vertex(cr['A'],cr['b'],cr['x'])regions[crid]=crnfound+=1log_(" Searching for adjacent regions\n")forkinrange(len(nonredundant)):n=cr['cind'][k]newactive=currentset['active'].copy()tryqpsearch=Trueifcr['ctype'][k]=='w':newactive[n]=Trueelifcr['ctype'][k]=='y':newactive[n]=Falsetryqpsearch=Falseelse:continuekeepgoing=Truewhilekeepgoing:newcrid=getcrid(newactive)ifnewcridnotinprobedregions:probedregions[newcrid]=[]newfullrank,newHiihalf=checkcrfullrank(dmpqp,newactive)ifnewfullrank:newcr={'active':newactive,'id':newcrid,'Hiihalf':newHiihalf}opensets,nopen=addcr(opensets,nopen,newcr)keepgoing=Falseelse:log_(f" {newcrid} is not full-rank\n")iftryqpsearch:tryqpsearch=Falselog_(" Trying QP neighbor search\n")cr['facet']=knewactive,keepgoing=findadjacentcr(cr,pmpqp)ifnotkeepgoing:log_(" No neighbor found\n")else:keepgoing=Falseelse:keepgoing=Falseelse:log_(f" {crid} is infeasible\n")log_(f" {nopen} regions open\n")log_(f"Finished with {nopen} open regions\n")log_()ifoptions['display']:elapsed=time.time()-starttimeprint(f"Found {nfound} regions ({nopen} left to search) after "f"{niter} iterations (in {elapsed:.1f} s).")# Optional plot.ifoptions['plot']andpmpqp['S'].shape[1]==2andnotos.getenv('OMIT_PLOTS')=='true':print("Making plot (may take some time).")plt.figure()fields=list(regions.keys())colors=viridiscolors(len(fields))cperm=np.argsort(np.sin(np.arange(1,len(fields)+1)))colors=colors[cperm]fori,finenumerate(fields):plt.fill(regions[f]['V'][:,0],regions[f]['V'][:,1],color=colors[i])plt.xlabel('p_1')plt.ylabel('p_2',rotation=0)# Return as list (mimicking MATLAB cell array).ifoptions['ascell']:result=[]forf,rinregions.items():region=r.copy()region['id']=fresult.append(region)returnresultreturnregions
# Example explicit MPC for a 2D system From "Optimal control of constrained# piecewise affine discrete-time systems" (Mayne and Rakovic, 2002).# [depends] functions/solvempqp.py# [makes] explicitvsimplicit.mat## [rule] copymatimportosimportsysimportnumpyasnpfromscipy.linalgimportsolve_discrete_arefromscipy.optimizeimportminimizeimporttimeimportmatplotlib.pyplotaspltfromscipy.ioimportsavematsys.path.append('functions/')fromfindXnimportfindXnfromss2mpqpimportss2mpqpfromsolvempqpimportsolvempqpfromhyperrectangleimporthyperrectangledef_dlqr(A,B,Q,R):"""Discrete-time LQR: returns (K, P) where u = K*x (negative gain)."""P=solve_discrete_are(A,B,Q,R)K=np.linalg.solve(R+B.T@P@B,B.T@P@A)return-K,P# Choose system.A=np.array([[1.,0.1],[0.,1.]])B=np.array([[0.],[0.0787]])Q=np.array([[1.,0.],[0.,0.]])R=0.1ulb=np.array([-1.])uub=np.array([1.])xlb=-np.inf*np.ones(2)xub=np.inf*np.ones(2)N=20Nx,Nu=B.shape# Find LQR terminal set.K,P=_dlqr(A,B,Q,R)Xn,Xnverts,_=findXn(A,B,K,N,xlb,xub,ulb,uub,'lqr',doplot=False)A_lqr=Xn[0]['A']b_lqr=Xn[0]['b']feas=Xn[-1]# X_N feasible setfeas['b']=np.asarray(feas['b']).flatten()# Get mpQP and solve.mpqp,_=ss2mpqp({'A':A,'B':B,'Q':Q,'R':R,'P':P,'N':N,'ulb':ulb,'uub':uub,'Af':A_lqr,'bf':b_lqr})regions=solvempqp(mpqp,plot=False)defpwalookup(x,regions,feas=None):"""Returns optimal first control z (or None if infeasible)."""z=Noneid_=''iffeasisNoneornp.all(feas['A']@x-feas['b']<=0):forregioninregions:ifnp.all(region['A']@x<=region['b']):z=region['Z']@x+region['z0']id_=region['id']breakreturnz,id_defsolve_qp(H,q,A_ineq,b_ineq):"""Solve QP: min 0.5 z'Hz + q'z s.t. A_ineq*z <= b_ineq."""H=0.5*(H+H.T)n=H.shape[0]res=minimize(fun=lambdaz:0.5*z@H@z+q@z,jac=lambdaz:H@z+q,x0=np.zeros(n),method='SLSQP',constraints={'type':'ineq','fun':lambdaz:b_ineq-A_ineq@z,'jac':lambdaz:-A_ineq},options={'ftol':1e-12,'maxiter':500,'disp':False},)returnres.x,res.success# Sample feasible points and measure lookup vs QP times.xbox=np.array([[-6.,6.],[-3.,3.]])Npts=10000Nmax=20000xpts=np.full((Nx,Npts),np.nan)lookuptimes=np.full(Npts,np.nan)qptimes=np.full(Npts,np.nan)jx=0np.random.seed(917)foriinrange(Nmax):xmin=xbox[:,0]dx=xbox[:,1]-xbox[:,0]xtry=xmin+dx*np.random.rand(Nx)ifnp.all(feas['A']@xtry-feas['b']<=0):jx+=1ifjx%500==1:print(f'Point {jx} of {Npts}')xpts[:,jx-1]=xtry# PWA lookup timing.t0=time.perf_counter()zlookup,_=pwalookup(xtry,regions,feas)lookuptimes[jx-1]=time.perf_counter()-t0# QP solve timing.t0=time.perf_counter()H_qp=mpqp['Q']q_qp=(mpqp['C']@xtry).flatten()A_qp=mpqp['A']b_qp=(mpqp['b']+mpqp['S']@xtry).flatten()zqp,qp_ok=solve_qp(H_qp,q_qp,A_qp,b_qp)qptimes[jx-1]=time.perf_counter()-t0ifnotqp_ok:print(f'QP failed at step {jx}!')ifzlookupisnotNoneandnp.max(np.abs(zqp-zlookup))>1e-5:print(f'Warning: Mismatch for point {jx}!')ifjx==Npts:breakprint(f'Sampled {jx} points.')xpts=xpts[:,:jx]lookuptimes=1000.*lookuptimes[:jx]# convert to msqptimes=1000.*qptimes[:jx]# convert to msdefkerneldensity(xsamples,sigma,x):"""Gaussian kernel density estimate."""xsamples=xsamples.flatten()x=x.reshape(1,-1)den=np.sqrt(2*np.pi)*sigma*len(xsamples)returnnp.sum(np.exp(-(x-xsamples.reshape(-1,1))**2/(2*sigma**2))/den,axis=0)sig=1.# kernel width in mstplot=np.linspace(0,100,1001)lookupdensity=kerneldensity(lookuptimes,sig,tplot)qpdensity=kerneldensity(qptimes,sig,tplot)ifnotos.getenv('OMIT_PLOTS')=='true':plt.figure()plt.plot(tplot,lookupdensity,'-r',tplot,qpdensity,'-g')plt.legend(['Explicit','Implicit'],loc='upper right')plt.title(f'{xpts.shape[1]} Samples')plt.xlabel('Solution Time (ms)')plt.ylabel('Samples')
