def solve_bvp_casadi(self):
"""
Uses casadi's interface to sundials to solve the boundary value
problem using a single-shooting method with automatic differen-
tiation.
Related to PCSJ code.
"""
self.bvpint = cs.Integrator('cvodes', self.modlT)
self.bvpint.setOption('abstol', self.intoptions['bvp_abstol'])
self.bvpint.setOption('reltol', self.intoptions['bvp_reltol'])
self.bvpint.setOption('tf', 1)
self.bvpint.setOption('disable_internal_warnings', True)
self.bvpint.setOption('fsens_err_con', True)
self.bvpint.init()
# Vector of unknowns [y0, T]
V = cs.MX.sym("V", self.neq + 1)
y0 = V[:-1]
T = V[-1]
param = cs.vertcat([self.param, T])
yf = self.bvpint.call(cs.integratorIn(x0=y0, p=param))[0]
fout = self.modlT.call(cs.daeIn(t=T, x=y0, p=param))[0]
# objective: continuity
obj = (yf -
y0)**2 # yf and y0 are the same ..i.e. 2 ends of periodic fcn
obj.append(
fout[0]) # y0 is a peak for state 0, i.e. fout[0] is slope state 0
#set up the matrix we want to solve
F = cs.MXFunction([V], [obj])
F.init()
guess = np.append(self.y0, self.T)
solver = cs.ImplicitFunction('kinsol', F)
solver.setOption('abstol', self.intoptions['bvp_ftol'])
solver.setOption('strategy', 'linesearch')
solver.setOption('exact_jacobian', False)
solver.setOption('pretype', 'both')
solver.setOption('use_preconditioner', True)
if self.intoptions['constraints'] == 'positive':
solver.setOption('constraints', (2, ) * (self.neq + 1))
solver.setOption('linear_solver_type', 'dense')
solver.init()
solver.setInput(guess)
solver.evaluate()
sol = solver.output().toArray().squeeze()
self.y0 = sol[:-1]
self.T = sol[-1]
def solveBVP_casadi(self):
"""
Uses casadi's interface to sundials to solve the boundary value
problem using a single-shooting method with automatic differen-
tiation.
"""
# Here we create and initialize the integrator SXFunction
self.bvpint = cs.CVodesIntegrator(self.modlT)
self.bvpint.setOption('abstol',self.intoptions['bvp_abstol'])
self.bvpint.setOption('reltol',self.intoptions['bvp_reltol'])
self.bvpint.setOption('tf',1)
self.bvpint.setOption('disable_internal_warnings', True)
self.bvpint.setOption('fsens_err_con', True)
self.bvpint.init()
# Vector of unknowns [y0, T]
V = cs.msym("V",self.NEQ+1)
y0 = V[:-1]
T = V[-1]
t = cs.msym('t')
param = cs.vertcat([self.paramset, T])
yf = self.bvpint.call(cs.integratorIn(x0=y0,p=param))[0]
fout = self.modlT.call(cs.daeIn(t=t, x=y0,p=param))[0]
obj = (yf - y0)**2
obj.append(fout[0])
F = cs.MXFunction([V],[obj])
F.init()
solver = cs.KinsolSolver(F)
solver.setOption('abstol',self.intoptions['bvp_ftol'])
solver.setOption('ad_mode', "forward")
solver.setOption('strategy','linesearch')
solver.setOption('numeric_jacobian', True)
solver.setOption('exact_jacobian', False)
solver.setOption('pretype', 'both')
solver.setOption('use_preconditioner', True)
solver.setOption('numeric_hessian', True)
solver.setOption('constraints', (2,)*(self.NEQ+1))
solver.setOption('verbose', False)
solver.setOption('sparse', False)
solver.setOption('linear_solver', 'dense')
solver.init()
solver.output().set(self.y0)
solver.solve()
self.y0 = solver.output().toArray().squeeze()
def defineOCP(self,
ocp,
DT=20,
controlCost=0,
xOpt=[],
uOpt=[],
finalStateCost=1,
deltaUCons=[]):
self.ocp = ocp
ocp = self.ocp
self.DT = DT
self.n_k = int(self.ocp.tf / self.DT)
self.controlCost = controlCost
stateScaling = C.vertcat([
ocp.variable(ocp.x[k].getName()).nominal
for k in range(ocp.x.size())
])
algStateScaling = C.vertcat([
ocp.variable(ocp.z[k].getName()).nominal
for k in range(ocp.z.size())
])
controlScaling = C.vertcat([
ocp.variable(ocp.u[k].getName()).nominal
for k in range(ocp.u.size())
])
xOpt = xOpt / stateScaling
uOpt = uOpt / controlScaling
self.xOpt = xOpt
self.uOpt = uOpt
self.stateScaling = C.vertcat([
ocp.variable(ocp.x[k].getName()).nominal
for k in range(ocp.x.size())
])
self.algStateScaling = C.vertcat([
ocp.variable(ocp.z[k].getName()).nominal
for k in range(ocp.z.size())
])
self.controlScaling = C.vertcat([
ocp.variable(ocp.u[k].getName()).nominal
for k in range(ocp.u.size())
])
odeS = C.substitute(
ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / stateScaling
algS = C.substitute(
ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
ltermS = C.substitute(
ocp.lterm, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
sysIn = C.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t)
sysOut = C.daeOut(ode=odeS, alg=algS, quad=ltermS)
odeF = C.SXFunction(sysIn, sysOut)
odeF.init()
C.Integrator.loadPlugin("idas")
G = C.Integrator("idas", odeF)
G.setOption("reltol", self.INTG_REL_TOL) #for CVODES and IDAS
G.setOption("abstol", self.INTG_ABS_TOL) #for CVODES and IDAS
G.setOption("max_multistep_order", 5) #for CVODES and IDAS
G.setOption("max_step_size", self.IDAS_MAX_STEP_SIZE) #for IDAS only
G.setOption("tf", self.DT)
self.G = G
#==============================================================================
# G.setOption('verbose',True)
# G.addMonitor('res')
# G.addMonitor('inputs')
# G.addMonitor('outputs')
#G.addMonitor('djacB')
# G.addMonitor('bjacB')
# G.addMonitor('jtimesB')
# G.addMonitor('psetup')
# G.addMonitor('psetupB')
# G.addMonitor('psolveB')
# G.addMonitor('resB')
# G.addMonitor('resS')
# G.addMonitor('rhsQB')
#==============================================================================
G.init()
self.n_u = self.ocp.u.size()
self.n_x = self.ocp.x.size()
self.n_v = self.n_u * self.n_k + self.n_x * self.n_k
self.V = C.MX.sym("V", int(self.n_v), 1)
self.U, self.X = self.splitVariables(self.V)
uMin = C.vertcat([
self.ocp.variable(self.ocp.u[i].getName()).min.getValue()
for i in range(self.n_u)
]) / controlScaling
uMax = C.vertcat([
self.ocp.variable(self.ocp.u[i].getName()).max.getValue()
for i in range(self.n_u)
]) / controlScaling
UMIN = C.vertcat([uMin for k in range(self.n_k)])
UMAX = C.vertcat([uMax for k in range(self.n_k)])
xMin = C.vertcat([
self.ocp.variable(self.ocp.x[i].getName()).min.getValue()
for i in range(self.n_x)
]) / stateScaling
xMax = C.vertcat([
self.ocp.variable(self.ocp.x[i].getName()).max.getValue()
for i in range(self.n_x)
]) / stateScaling
XMIN = C.vertcat([xMin for k in range(self.n_k)])
XMAX = C.vertcat([xMax for k in range(self.n_k)])
if len(deltaUCons) > 0:
addDeltaUCons = True
deltaUCons = deltaUCons / self.controlScaling
else:
addDeltaUCons = False
pathIn = C.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t)
pathVarNames = [sv.getName() for sv in ocp.beq(ocp.path)]
pathScaling = C.vertcat([ocp.nominal(pv) for pv in pathVarNames])
pathS = C.substitute(
ocp.beq(ocp.beq(ocp.path)), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / pathScaling
pathConstraints = C.SXFunction(pathIn, [pathS])
pathMax = C.vertcat([
ocp.variable(pathVarNames[i]).max.getValue()
for i in range(ocp.path.size())
]) / pathScaling
pathMin = C.vertcat([
ocp.variable(pathVarNames[i]).min.getValue()
for i in range(ocp.path.size())
]) / pathScaling
pathConstraints.setOption("name", "PATH")
pathConstraints.init()
pathConstraints.setInput(xOpt, 'x')
pathConstraints.setInput([], 'z')
pathConstraints.setInput(uOpt, 'p')
pathConstraints.setInput(0, 't')
pathConstraints.evaluate()
pathOpt = pathConstraints.getOutput()
optimalValues = {}
print 'min <= (name,optimal,nominal) <= max'
for i in range(self.n_x):
print ocp.variable(
ocp.x[i].getName()).min.getValue(), ' <= (', ocp.x[i].getName(
), ',', xOpt[i] * stateScaling[i], ',', stateScaling[
i], ') <= ', ocp.variable(
ocp.x[i].getName()).max.getValue()
optimalValues[ocp.x[i].getName()] = xOpt[i] * stateScaling[i]
for i in range(self.n_u):
print ocp.variable(
ocp.u[i].getName()).min.getValue(), ' <= (', ocp.u[i].getName(
), ',', uOpt[i] * controlScaling[i], ',', controlScaling[
i], ') <= ', ocp.variable(
ocp.u[i].getName()).max.getValue()
if addDeltaUCons:
print -deltaUCons[i] * controlScaling[i], ' <= (Delta(', ocp.u[
i].getName(), ')/DTMPC,', 0, ',', controlScaling[
i], ') <= ', deltaUCons[i] * controlScaling[i]
optimalValues[ocp.u[i].getName()] = uOpt[i] * controlScaling[i]
for i in range(len(pathVarNames)):
print ocp.variable(pathVarNames[i]).min.getValue(
), ' <= (', pathVarNames[i], ',', pathOpt[i] * pathScaling[
i], ',', pathScaling[i], ') <= ', ocp.variable(
pathVarNames[i]).max.getValue()
optimalValues[pathVarNames[i]] = pathOpt[i] * pathScaling[i]
plotTags = [ocp.x[i].getName() for i in range(ocp.x.size())]
plotTags = plotTags + [ocp.u[i].getName() for i in range(ocp.u.size())]
plotTags = plotTags + [sv.getName() for sv in ocp.beq(ocp.path)]
self.plotTags = plotTags
self.optimalValues = optimalValues
# Constraint functions
g = []
g_min = []
g_max = []
self.XU0 = C.MX.sym("XU0", self.n_x + self.n_u, 1)
Z = self.XU0[0:self.n_x]
U0 = self.XU0[self.n_x:self.n_x + self.n_u]
# Build up a graph of integrator calls
obj = 0
zf = C.vertcat([
ocp.variable(ocp.z[k].getName()).start for k in range(ocp.z.size())
]) / algStateScaling
for k in range(self.n_k):
Z, QF, zf = C.integratorOut(
G(C.integratorIn(x0=Z, p=self.U[k], z0=zf)), "xf", "qf", "zf")
errU = self.U[k] - U0
obj = obj + QF + C.mul(C.mul(errU.T, controlCost), errU)
U0 = self.U[k]
# include MS constraints!
g.append(Z - self.X[k])
g_min.append(NP.zeros(self.n_x))
g_max.append(NP.zeros(self.n_x))
Z = self.X[k]
[pathCons] = pathConstraints.call(
C.daeIn(t=[], x=self.X[k], z=zf, p=self.U[k]))
g.append(pathCons) ## be carefull on giving all inputs
g_max.append(pathMax)
g_min.append(pathMin)
if addDeltaUCons:
g.append(errU)
g_max.append(deltaUCons * DT)
g_min.append(-deltaUCons * DT)
#errU = (self.U[-1]-uOpt)
#errX = self.X[-1]-xOpt
#obj = obj + finalStateCost*C.mul((errX).trans(),(errX))+C.mul(C.mul(errU.T,controlCost),errU)
self.obj = obj
### Constrains
g = C.vertcat(g)
nlp = C.MXFunction(C.nlpIn(x=self.V, p=self.XU0), C.nlpOut(f=obj, g=g))
nlp.init()
self.odeF = odeF
self.nlp = nlp
solver = C.NlpSolver('ipopt', nlp)
# remove the comment to implement the hessian
solver.setOption('hessian_approximation',
'limited-memory') # comment for exact hessian
solver.setOption('print_user_options', 'no')
solver.setOption("tol", self.IPOPT_tol) # IPOPT tolerance
solver.setOption("dual_inf_tol",
self.IPOPT_dual_inf_tol) # dual infeasibility
solver.setOption("constr_viol_tol",
self.IPOPT_constr_viol_tol) # primal infeasibility
solver.setOption("compl_inf_tol",
self.IPOPT_compl_inf_tol) # complementarity
# solver.setOption("acceptable_tol",0.01)
# solver.setOption("acceptable_obj_change_tol",1e-6)
# solver.setOption("acceptable_constr_viol_tol",1e-6)
solver.setOption("max_iter",
self.IPOPT_max_iter) # IPOPT maximum iterations
solver.setOption("print_level", self.IPOPT_print_level)
solver.setOption("max_cpu_time",
self.IPOPT_max_cpu_time) # IPOPT maximum iterations
solver.init()
### Variable Bounds and initial guess
solver.setInput(C.vertcat([UMIN, XMIN]), 'lbx') # u_L
solver.setInput(C.vertcat([UMAX, XMAX]), 'ubx') # u_U
solver.setInput(C.vertcat(g_min), 'lbg') # g_L
solver.setInput(C.vertcat(g_max), 'ubg') # g_U
self.solver = solver
u0N = C.vertcat([
self.ocp.variable(self.ocp.u[i].getName()).initialGuess.getValue()
for i in range(self.n_u)
]) / controlScaling
x0N = C.vertcat([
self.ocp.variable(self.ocp.x[i].getName()).initialGuess.getValue()
for i in range(self.n_x)
]) / stateScaling
USOL, XSOL = self.forwardSimulation(x0N, u0N)
self.USOL = USOL
self.XSOL = XSOL