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')
from pomodoro.solver.solver2 import Solver2 from pomodoro.discs.expression import Expression from casadi import * mu_m = 1.6# per day Ks = 7.5 rho_m = 0.10 Q0 = 0.04 Sin = 4.0 tph = 3600.0 x0 = NP.array([7.0,0.1,40]) x10 = NP.array([6.0,0.1,30]) prob = MultipleShootingProblem() tend = prob.setTimeRange(0.0,7.0) x = prob.addStates(3,[0.0,0.0,0.0],[1000.0,1.0,1000.0],[0.14,0.04,100.],method=['LagrangeStates',7]) u = prob.addControls(1,[0],[0.5],method=['PiecewiseConstant',7]) S = x[0]; Q = x[1]; X = x[2] rho = rho_m*S/(S + Ks) mu = mu_m*(1 - (Q0/Q)) rhs = SX.zeros(3) rhs[0] = -rho*X - u[0]*(S - Sin) rhs[1] = rho - mu*Q rhs[2] = mu*X - u[0]*X prob.addOde(x,u,rhs) f = sum((x('control')[2]-100)**2) prob.addObjective(f) prob.setInitCondition(x0)
S00 = 0.2 S11 = 1.0 S22 = 0.5 S33 = 0.2 S44 = 0.5000 S55 = 0.0000005 r0 = NP.array([2.14, 1.09, 114.2, 112.9, 14.19, -1113.5]) r1 = NP.array([2.9805,0.9612,106.0,100.75,18.038,-4565.88]) r2 = NP.array([3.5176,0.7395,87.0,79.8,8.256,-6239.33]) tph = 3600.0 x0 = NP.array([1.0,0.5,100.0,100.0]) prob = MultipleShootingProblem() tend = prob.setTimeRange(0.0,3000.0) x = prob.addStates(4,[0.0,0.0,50.0,50.0],[10.0,10.0,250.0,250.0],[2.14,1.09,114.2,112.9],method=['LagrangeStates',50]) #prob.addConstraints(x(0),x0) #theta0 = prob.addParameters(1,100.0,110.0) u = prob.addControls(2,[3.0,-9000.0],[35.0,0.0],method=['PiecewiseConstant',50]) #u.load('control_in_fix.txt') #u.plot() #theta0 = prob.addControls(1,[90.0],[120.0],method=['PiecewiseConstant',50]) #theta0.load(NP.random.normal(104.9,5,50)) #theta0.fix() rhs = SX.zeros(4) cA = x[0]
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')