def solve(self): I = self.InitIntegrate(self.MPC.plant_model, self.MPC.t_plant) if self.Estimator is not None: est_P = NP.reshape(self.Estimator.P00, self.MPC.nx * self.MPC.nx) else: est_P = 0.0 self.x_final[:, 0] = self.MPC.x0 x_plant = self.MPC.x0 self.est = EstimationRoutine(self) for self.k in range(self.MPC.N_sampling): i = self.k print i print self.MPC.N_sampling self.MPC.x_con.init(x_plant) self.MPC.x_init.setPVals(self.x_final[:, i]) #print self.MPC.x_init.getPVals() #TRY self.solveOCP(self.MPC.prob) self.u_final[:, i] = self.MPC.u_con(0, True).flatten() I.setInput(self.u_final[:, i], 'p') #print self.u_final[:,i] xint0 = x_plant for j in range(self.Nintegrate): I.setInput(xint0, 'x0') I.evaluate() xint0 = I.getOutput('xf') x_plant = NP.array(xint0).flatten() x_meas = self.est.getMeasurements(x_plant) self.x_measured[:, i + 1] = x_meas self.x_final[:, i + 1], est_P = self.est.estimate( x_meas, est_P, self.x_final[:, i], self.u_final[:, i], self.MPC.t_plant) #raw_input('TTT') if self.save is not None: NP.savetxt(self.save + '_states', self.x_final) NP.savetxt(self.save + '_controls', self.u_final)
def solve(self): I = self.InitIntegrate(self.MPC.plant_model,self.MPC.t_plant) if self.Estimator is not None: est_P = NP.reshape(self.Estimator.P00,self.MPC.nx*self.MPC.nx) else: est_P = 0.0 self.x_final[:,0] = self.MPC.x0 x_plant = self.MPC.x0 self.est = EstimationRoutine(self) for self.k in range(self.MPC.N_sampling): i = self.k print i print self.MPC.N_sampling self.MPC.x_con.init(x_plant) self.MPC.x_init.setPVals(self.x_final[:,i]) #print self.MPC.x_init.getPVals() #TRY self.solveOCP(self.MPC.prob) self.u_final[:,i] = self.MPC.u_con(0,True).flatten() I.setInput(self.u_final[:,i],'p') #print self.u_final[:,i] xint0 = x_plant for j in range(self.Nintegrate): I.setInput(xint0,'x0') I.evaluate() xint0 = I.getOutput('xf') x_plant = NP.array(xint0).flatten() x_meas = self.est.getMeasurements(x_plant) self.x_measured[:,i+1] = x_meas self.x_final[:,i+1],est_P = self.est.estimate(x_meas,est_P,self.x_final[:,i],self.u_final[:,i],self.MPC.t_plant) #raw_input('TTT') if self.save is not None: NP.savetxt(self.save+'_states',self.x_final) NP.savetxt(self.save+'_controls',self.u_final)
class MPCsolve(object): def __init__(self, MPC, Estimator=None, printlevel=0, max_iter=4000, save=None): self.MPC = MPC self.Estimator = Estimator self.printlevel = printlevel self.max_iter = max_iter if self.MPC.nmx is None: self.nmx = list(xrange(self.MPC.nx)) self.std = self.MPC.nx * [0] else: self.nmx = self.MPC.nmx self.std = self.MPC.std self.x_final = NP.zeros((self.MPC.nx, MPC.N_sampling + 1)) self.x_measured = NP.zeros((len(self.nmx), MPC.N_sampling + 1)) self.u_final = NP.zeros((MPC.nu, MPC.N_sampling)) self.save = save if save is not None and isinstance(save, str) is False: raise IOError('File Name to save should be a string') def solveOCP(self, prob, printlevel=0, max_iter=4000): solver = Solver2(prob, printlevel=self.printlevel, max_iter=self.max_iter) solver.solve() def InitIntegrate(self, f, tfinal, Nintegrate=1): self.Nintegrate = Nintegrate deltat_int = tfinal / Nintegrate I = Integrator('cvodes', f) I.setOption('tf', deltat_int) I.setOption('abstol', 1e-4) I.setOption('reltol', 1e-4) I.init() return I def solve(self): I = self.InitIntegrate(self.MPC.plant_model, self.MPC.t_plant) if self.Estimator is not None: est_P = NP.reshape(self.Estimator.P00, self.MPC.nx * self.MPC.nx) else: est_P = 0.0 self.x_final[:, 0] = self.MPC.x0 x_plant = self.MPC.x0 self.est = EstimationRoutine(self) for self.k in range(self.MPC.N_sampling): i = self.k print i print self.MPC.N_sampling self.MPC.x_con.init(x_plant) self.MPC.x_init.setPVals(self.x_final[:, i]) #print self.MPC.x_init.getPVals() #TRY self.solveOCP(self.MPC.prob) self.u_final[:, i] = self.MPC.u_con(0, True).flatten() I.setInput(self.u_final[:, i], 'p') #print self.u_final[:,i] xint0 = x_plant for j in range(self.Nintegrate): I.setInput(xint0, 'x0') I.evaluate() xint0 = I.getOutput('xf') x_plant = NP.array(xint0).flatten() x_meas = self.est.getMeasurements(x_plant) self.x_measured[:, i + 1] = x_meas self.x_final[:, i + 1], est_P = self.est.estimate( x_meas, est_P, self.x_final[:, i], self.u_final[:, i], self.MPC.t_plant) #raw_input('TTT') if self.save is not None: NP.savetxt(self.save + '_states', self.x_final) NP.savetxt(self.save + '_controls', self.u_final) def plotStates(self, n=None, save=None): t = NP.linspace(0.0, self.MPC.total_plant, self.MPC.N_sampling + 1) if n is None: np = NP.ceil(NP.sqrt(self.MPC.nx)) for i in range(self.MPC.nx): plt.subplot(np, np, i + 1) plt.plot(t, self.x_final[i, :]) plt.xlabel('t') plt.ylabel('x' + str(i)) if save is not None: plt.savefig(save[0] + '_states', format=save[1]) plt.show() elif isinstance(n, list) is True: if len(n) > self.MPC.nx: raise IOError('List of states too long') np = NP.ceil(NP.sqrt(NP.abs(len(n)))) j = 1 for i in n: plt.subplot(np, np, j) plt.plot(t, self.x_final[i, :]) plt.xlabel('t') plt.ylabel('x' + str(i)) j += 1 if save is not None: plt.savefig(save[0] + '_states', format=save[1]) plt.show() elif isinstance(n, int) is True: plt.plot(t, self.x_final[n, :]) plt.xlabel('t') plt.ylabel('x' + str(n)) if save is not None: plt.savefig(save[0] + '_states', format=save[1]) plt.show() else: raise IOError('\'n\' should be a list or integer') def plotControls(self, n=None, save=None): t = NP.linspace(0.0, self.MPC.total_plant - self.MPC.t_plant, self.MPC.N_sampling) if n is None: np = NP.ceil(NP.sqrt(NP.abs(self.MPC.nu))) for i in range(self.MPC.nu): plt.subplot(np, np, i + 1) plt.step(t, self.u_final[i, :]) plt.xlabel('t') plt.ylabel('u' + str(i)) if save is not None: plt.savefig(save[0] + 'controls', format=save[1]) plt.show() elif isinstance(n, list) is True: if len(n) > self.MPC.nu: raise IOError('List of controls too long') np = NP.ceil(NP.sqrt(NP.abs(len(n)))) j = 1 for i in n: plt.subplot(np, np, j) plt.plot(t, self.u_final[i, :]) plt.xlabel('t') plt.ylabel('u' + str(i)) j += 1 if save is not None: plt.savefig(save[0] + '_controls', format=save[1]) plt.show() elif isinstance(n, int) is True: plt.plot(t, self.u_final[n, :]) plt.xlabel('t') plt.ylabel('u' + str(n)) if save is not None: plt.savefig(save[0] + '_controls', format=save[1]) plt.show() else: raise IOError('\'n\' should be a list or integer') def plotMeasurements(self): t = NP.linspace(0.0, self.MPC.total_plant, self.MPC.N_sampling + 1) i = 0 for j in self.nmx: plt.plot(t, self.x_measured[i, :]) plt.plot(t, self.x_final[j, :]) i += 1 plt.show()
class MPCsolve(object): def __init__(self,MPC,Estimator=None,printlevel=0,max_iter=4000,save=None): self.MPC = MPC self.Estimator = Estimator self.printlevel = printlevel self.max_iter = max_iter if self.MPC.nmx is None: self.nmx = list(xrange(self.MPC.nx)) self.std = self.MPC.nx*[0] else: self.nmx = self.MPC.nmx self.std = self.MPC.std self.x_final = NP.zeros((self.MPC.nx,MPC.N_sampling+1)) self.x_measured = NP.zeros((len(self.nmx),MPC.N_sampling+1)) self.u_final = NP.zeros((MPC.nu,MPC.N_sampling)) self.save = save if save is not None and isinstance(save,str) is False: raise IOError('File Name to save should be a string') def solveOCP(self,prob,printlevel=0,max_iter=4000): solver = Solver2(prob,printlevel=self.printlevel,max_iter=self.max_iter) solver.solve() def InitIntegrate(self,f,tfinal,Nintegrate = 1): self.Nintegrate = Nintegrate deltat_int = tfinal/Nintegrate I = Integrator('cvodes',f) I.setOption('tf',deltat_int) I.setOption('abstol',1e-4) I.setOption('reltol',1e-4) I.init() return I def solve(self): I = self.InitIntegrate(self.MPC.plant_model,self.MPC.t_plant) if self.Estimator is not None: est_P = NP.reshape(self.Estimator.P00,self.MPC.nx*self.MPC.nx) else: est_P = 0.0 self.x_final[:,0] = self.MPC.x0 x_plant = self.MPC.x0 self.est = EstimationRoutine(self) for self.k in range(self.MPC.N_sampling): i = self.k print i print self.MPC.N_sampling self.MPC.x_con.init(x_plant) self.MPC.x_init.setPVals(self.x_final[:,i]) #print self.MPC.x_init.getPVals() #TRY self.solveOCP(self.MPC.prob) self.u_final[:,i] = self.MPC.u_con(0,True).flatten() I.setInput(self.u_final[:,i],'p') #print self.u_final[:,i] xint0 = x_plant for j in range(self.Nintegrate): I.setInput(xint0,'x0') I.evaluate() xint0 = I.getOutput('xf') x_plant = NP.array(xint0).flatten() x_meas = self.est.getMeasurements(x_plant) self.x_measured[:,i+1] = x_meas self.x_final[:,i+1],est_P = self.est.estimate(x_meas,est_P,self.x_final[:,i],self.u_final[:,i],self.MPC.t_plant) #raw_input('TTT') if self.save is not None: NP.savetxt(self.save+'_states',self.x_final) NP.savetxt(self.save+'_controls',self.u_final) def plotStates(self,n=None,save=None): t = NP.linspace(0.0,self.MPC.total_plant,self.MPC.N_sampling+1) if n is None: np = NP.ceil(NP.sqrt(self.MPC.nx)) for i in range(self.MPC.nx): plt.subplot(np,np,i+1) plt.plot(t,self.x_final[i,:]) plt.xlabel('t') plt.ylabel('x'+str(i)) if save is not None: plt.savefig(save[0]+'_states',format=save[1]) plt.show() elif isinstance(n,list) is True: if len(n)>self.MPC.nx: raise IOError('List of states too long') np = NP.ceil(NP.sqrt(NP.abs(len(n)))) j = 1 for i in n: plt.subplot(np,np,j) plt.plot(t,self.x_final[i,:]) plt.xlabel('t') plt.ylabel('x'+str(i)) j+=1 if save is not None: plt.savefig(save[0]+'_states',format=save[1]) plt.show() elif isinstance(n,int) is True: plt.plot(t,self.x_final[n,:]) plt.xlabel('t') plt.ylabel('x'+str(n)) if save is not None: plt.savefig(save[0]+'_states',format=save[1]) plt.show() else: raise IOError('\'n\' should be a list or integer') def plotControls(self,n=None,save=None): t = NP.linspace(0.0,self.MPC.total_plant-self.MPC.t_plant,self.MPC.N_sampling) if n is None: np = NP.ceil(NP.sqrt(NP.abs(self.MPC.nu))) for i in range(self.MPC.nu): plt.subplot(np,np,i+1) plt.step(t,self.u_final[i,:]) plt.xlabel('t') plt.ylabel('u'+str(i)) if save is not None: plt.savefig(save[0]+'controls',format=save[1]) plt.show() elif isinstance(n,list) is True: if len(n)>self.MPC.nu: raise IOError('List of controls too long') np = NP.ceil(NP.sqrt(NP.abs(len(n)))) j = 1 for i in n: plt.subplot(np,np,j) plt.plot(t,self.u_final[i,:]) plt.xlabel('t') plt.ylabel('u'+str(i)) j+=1 if save is not None: plt.savefig(save[0]+'_controls',format=save[1]) plt.show() elif isinstance(n,int) is True: plt.plot(t,self.u_final[n,:]) plt.xlabel('t') plt.ylabel('u'+str(n)) if save is not None: plt.savefig(save[0]+'_controls',format=save[1]) plt.show() else: raise IOError('\'n\' should be a list or integer') def plotMeasurements(self): t = NP.linspace(0.0,self.MPC.total_plant,self.MPC.N_sampling+1) i = 0 for j in self.nmx: plt.plot(t,self.x_measured[i,:]) plt.plot(t,self.x_final[j,:]) i+=1 plt.show()