r = r0 X0 = x[0]-r[0] X1 = x[1]-r[1] X2 = x[2]-r[2] X3 = x[3]-r[3] U1 = u[0]-r[4] U2 = u[1]-r[5] rhs[0] = (1/tph)*(u[0]*(cA0-cA) - k1*cA - k3*cA*cA) rhs[1] = (1/tph)* (- u[0]*cB + k1*cA - k2*cB) rhs[2] = (1/tph)*(u[0]*(theta0-theta) - (1/(rho*Cp)) *(k1*cA*H1 + k2*cB*H2 + k3*cA*cA*H3)+(kw*AR/(rho*Cp*VR))*(thetaK -theta)) rhs[3] = (1/tph)*((1/(mK*CPK))*(u[1] + kw*AR*(theta-thetaK))) #rhs[4] = S00*X0*X0 + S11*X1*X1 + S22*X2*X2 + S33*X3*X3 + S44*U1*U1 + S55*U2*U2 prob.addOde(x,u,rhs) r0 = NP.array([2.14, 1.09, 114.2, 112.9, 14.19, -1113.5]) f = sum(S00*(x('control')[0]-r0[0])**2+S11*(x('control')[1]-r0[1])**2 +S22*(x('control')[2]-r0[2])**2+S33*(x('control')[3]-r0[3])**2 )+sum(S44*(u('control')[0]-r0[4])**2 + S55*(u('control')[1]-r0[5])**2)#print x0 prob.addObjective(f) prob.setInitCondition(x0) solver = SolveMS(prob) solver.solve() x.plot() #solver.solve() ##f = x[4](-1) #f = sum(S00*X0('coll')*X0('coll') + S11*X1('coll')*X1('coll') + S22*X2('coll')*X2('coll') + S33*X3('coll')*X3('coll') + S44*U1('coll')*U1('coll') + S55*U2('coll')*U2('coll')) ##f = 0.0 #prob.addObjective(f) # #solver = Solver2(prob,printlevel=5,max_iter=10000) ##plist = NP.array([[1.1,1.2,1.3,1.4]]) #print time.time()-t #solver.solve()
class MPCproblem(object): ## Initializing the controller. Time Ranges etc. def __init__(self,N_sampling,t_control=None,total_plant=None,dis='Collocation'): self.N_sampling = N_sampling self.dis = dis if t_control == None and total_plant == None: raise IOError('Define either control horizon or total plant run time') if t_control is not None: self.t_plant = t_control/10.0 #10 Discretizations fixed self.t_control = t_control self.total_plant = self.t_plant * self.N_sampling if total_plant is not None: self.total_plant = total_plant self.t_plant = total_plant/self.N_sampling self.t_control = self.t_plant*10 #10 Discretizations fixed if total_plant is not None and t_control is not None: if total_plant/self.N_sampling > t_control/10: raise IOError('Check control horizon and total plant run time') if dis is not 'MultipleShooting': self.prob = Problem() else: print "Hello" self.prob = MultipleShootingProblem() self.prob.setTimeRange(0.0,self.t_control) self.t = SX.sym('t') self.nmx = None def addControllerStates(self,n,xmin,xmax,xstart=None,method=['LagrangeColl',10],constr=True): if self.dis is not 'MultipleShooting': self.x_con = self.prob.addStates(n,xmin,xmax,xstart,method=['LagrangeColl',10]) self.x_init = self.prob.addFixedParameters(n) self.prob.addConstraints(self.x_con(0)-self.x_init,0.0) self.nx = n else: self.x_con = self.prob.addStates(n,xmin,xmax,xstart,method=['LagrangeColl',10]) return self.x_con def addControllerInputs(self,n,umin,umax,ustart=None,method=['PiecewiseConstant',10]): self.u_con = self.prob.addControls(n,umin,umax,ustart=None,method=['PiecewiseConstant',10]) self.nu = n return self.u_con def addControllerODEs(self,diff,u,rhs,colsel=None): self.rhs_con = rhs if self.dis is not 'MultipleShooting': self.prob.addOde(self.x_con,self.rhs_con) else: self.prob.addOde(self.x_con,self.u_con,self.rhs_con) def addControllerConstraints(self,c,cmin,cmax1=None): self.prob.addConstraints(c,cmin,cmax=cmax1) def addControlObjective(self,f): self.prob.addObjective(f) ### Initialize the plant def addPlantStates(self,n): self.x_plant = SX.sym('x',n) return self.x_plant def addPlantInputs(self,n): self.u_plant = SX.sym('u',n) return self.u_plant def addPlantODEs(self,x,rhs): self.plant_model = SXFunction(daeIn(x=self.x_plant,p=self.u_plant,t=self.t),daeOut(ode=rhs)) self.plant_model.setOption('name','Integrator') def setInitCondition(self,xint): if issubclass(type(xint),numpy.ndarray) is False: raise IOError('Initial Condition should be an array') self.x0 = xint def addMeasurementNoise(self,nmx,std): self.nmx = nmx self.std = std if len(nmx) != len(std): raise IOError('Standard Deviation For All Measurements')
class MPCproblem(object): ## Initializing the controller. Time Ranges etc. def __init__(self, N_sampling, t_control=None, total_plant=None, dis='Collocation'): self.N_sampling = N_sampling self.dis = dis if t_control == None and total_plant == None: raise IOError( 'Define either control horizon or total plant run time') if t_control is not None: self.t_plant = t_control / 10.0 #10 Discretizations fixed self.t_control = t_control self.total_plant = self.t_plant * self.N_sampling if total_plant is not None: self.total_plant = total_plant self.t_plant = total_plant / self.N_sampling self.t_control = self.t_plant * 10 #10 Discretizations fixed if total_plant is not None and t_control is not None: if total_plant / self.N_sampling > t_control / 10: raise IOError('Check control horizon and total plant run time') if dis is not 'MultipleShooting': self.prob = Problem() else: print "Hello" self.prob = MultipleShootingProblem() self.prob.setTimeRange(0.0, self.t_control) self.t = SX.sym('t') self.nmx = None def addControllerStates(self, n, xmin, xmax, xstart=None, method=['LagrangeColl', 10], constr=True): if self.dis is not 'MultipleShooting': self.x_con = self.prob.addStates(n, xmin, xmax, xstart, method=['LagrangeColl', 10]) self.x_init = self.prob.addFixedParameters(n) self.prob.addConstraints(self.x_con(0) - self.x_init, 0.0) self.nx = n else: self.x_con = self.prob.addStates(n, xmin, xmax, xstart, method=['LagrangeColl', 10]) return self.x_con def addControllerInputs(self, n, umin, umax, ustart=None, method=['PiecewiseConstant', 10]): self.u_con = self.prob.addControls(n, umin, umax, ustart=None, method=['PiecewiseConstant', 10]) self.nu = n return self.u_con def addControllerODEs(self, diff, u, rhs, colsel=None): self.rhs_con = rhs if self.dis is not 'MultipleShooting': self.prob.addOde(self.x_con, self.rhs_con) else: self.prob.addOde(self.x_con, self.u_con, self.rhs_con) def addControllerConstraints(self, c, cmin, cmax1=None): self.prob.addConstraints(c, cmin, cmax=cmax1) def addControlObjective(self, f): self.prob.addObjective(f) ### Initialize the plant def addPlantStates(self, n): self.x_plant = SX.sym('x', n) return self.x_plant def addPlantInputs(self, n): self.u_plant = SX.sym('u', n) return self.u_plant def addPlantODEs(self, x, rhs): self.plant_model = SXFunction( daeIn(x=self.x_plant, p=self.u_plant, t=self.t), daeOut(ode=rhs)) self.plant_model.setOption('name', 'Integrator') def setInitCondition(self, xint): if issubclass(type(xint), numpy.ndarray) is False: raise IOError('Initial Condition should be an array') self.x0 = xint def addMeasurementNoise(self, nmx, std): self.nmx = nmx self.std = std if len(nmx) != len(std): raise IOError('Standard Deviation For All Measurements')