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')
