def __init__(self, dae, nk=None, nicp=1, deg=4, collPoly='RADAU'):
assert(nk is not None)
assert(isinstance(dae, Dae))
self.dae = dae
self.nk = nk
self.nicp = nicp
self.deg = deg
self.collPoly = collPoly
self._xBounds = dict( [(name,[None]*(nk+1)) for name in dae.xNames()] )
self._zBounds = dict( [(name,[None]*nk) for name in dae.zNames()] )
self._uBounds = dict( [(name,[None]*nk) for name in dae.uNames()] )
self._pBounds = dict( [(name,None) for name in dae.pNames()] )
self._xGuess = dict( [(name,[None]*(nk+1)) for name in dae.xNames()] )
self._zGuess = dict( [(name,[None]*nk) for name in dae.zNames()] )
self._uGuess = dict( [(name,[None]*nk) for name in dae.uNames()] )
self._pGuess = dict( [(name,None) for name in dae.pNames()] )
self._constraints = Constraints()
# set (V,XD,XA,U,P)
self._setupDesignVars()
def __init__(self, dae, nSteps):
# check inputs
assert isinstance(dae, Dae)
assert isinstance(nSteps, int)
# make sure dae has everything
assert hasattr(dae, '_residual')
self.dae = dae
self.dae._freeze('MultipleShootingStage(dae)')
# set up design vars
self.nSteps = nSteps
self.states = C.msym("x", self.nStates(), self.nSteps)
self.actions = C.msym("u", self.nActions(), self.nSteps)
self.params = C.msym("p", self.nParams())
# self._dvs = C.msym("dv",self.nStates()*self.nSteps+self.nActions()*self.nSteps+self.nParams())
#
# numXVars = self.nStates()*self.nSteps
# numUVars = self.nActions()*self.nSteps
# numPVars = self.nParams()
#
# self.states = C.reshape(self._dvs[:numXVars], [self.nStates(), self.nSteps])
# self.actions = C.reshape(self._dvs[numXVars:numXVars+numUVars], [self.nActions(), self.nSteps])
# self.params = self._dvs[numXVars+numUVars:]
#
# assert self.states.size() == numXVars
# assert self.actions.size() == numUVars
# assert self.params.size() == numPVars
# set up interface
self._constraints = Constraints()
self._bounds = Bounds(self.dae._xNames, self.dae._uNames,
self.dae._pNames, self.nSteps)
self._initialGuess = InitialGuess(self.dae._xNames, self.dae._uNames,
self.dae._pNames, self.nSteps)
self._designVars = DesignVars(
(self.dae._xNames, self.states), (self.dae._uNames, self.actions),
(self.dae._pNames, self.params), self.nSteps)
def __init__(self,dae,nk):
self.dae = dae
self.nk = nk
self._gaussNewtonObjF = []
mapSize = len(self.dae.xNames())*(self.nk+1) + len(self.dae.pNames())
V = C.msym('dvs',mapSize)
self._dvMap = nmheMaps.VectorizedReadOnlyNmheMap(self.dae,self.nk,V)
self._boundMap = nmheMaps.WriteableNmheMap(self.dae,self.nk)
self._guessMap = nmheMaps.WriteableNmheMap(self.dae,self.nk)
self._U = C.msym('u',self.nk,len(self.dae.uNames()))
self._outputMapGenerator = nmheMaps.NmheOutputMapGenerator(self,self._U)
self._outputMap = nmheMaps.NmheOutputMap(self._outputMapGenerator, self._dvMap.vectorize(), self._U)
self._constraints = Constraints()
def __init__(self, dae, nSteps):
# check inputs
assert isinstance(dae, Dae)
assert isinstance(nSteps, int)
# make sure dae has everything
assert hasattr(dae,'_residual')
self.dae = dae
self.dae._freeze('MultipleShootingStage(dae)')
# set up design vars
self.nSteps = nSteps
self.states = C.msym("x" ,self.nStates(),self.nSteps)
self.actions = C.msym("u",self.nActions(),self.nSteps)
self.params = C.msym("p",self.nParams())
# self._dvs = C.msym("dv",self.nStates()*self.nSteps+self.nActions()*self.nSteps+self.nParams())
#
# numXVars = self.nStates()*self.nSteps
# numUVars = self.nActions()*self.nSteps
# numPVars = self.nParams()
#
# self.states = C.reshape(self._dvs[:numXVars], [self.nStates(), self.nSteps])
# self.actions = C.reshape(self._dvs[numXVars:numXVars+numUVars], [self.nActions(), self.nSteps])
# self.params = self._dvs[numXVars+numUVars:]
#
# assert self.states.size() == numXVars
# assert self.actions.size() == numUVars
# assert self.params.size() == numPVars
# set up interface
self._constraints = Constraints()
self._bounds = Bounds(self.dae._xNames, self.dae._uNames, self.dae._pNames, self.nSteps)
self._initialGuess = InitialGuess(self.dae._xNames, self.dae._uNames, self.dae._pNames, self.nSteps)
self._designVars = DesignVars((self.dae._xNames,self.states),
(self.dae._uNames,self.actions),
(self.dae._pNames,self.params),
self.nSteps)
class MultipleShootingStage():
def __init__(self, dae, nSteps):
# check inputs
assert isinstance(dae, Dae)
assert isinstance(nSteps, int)
# make sure dae has everything
assert hasattr(dae,'_residual')
self.dae = dae
self.dae._freeze('MultipleShootingStage(dae)')
# set up design vars
self.nSteps = nSteps
self.states = C.msym("x" ,self.nStates(),self.nSteps)
self.actions = C.msym("u",self.nActions(),self.nSteps)
self.params = C.msym("p",self.nParams())
# self._dvs = C.msym("dv",self.nStates()*self.nSteps+self.nActions()*self.nSteps+self.nParams())
#
# numXVars = self.nStates()*self.nSteps
# numUVars = self.nActions()*self.nSteps
# numPVars = self.nParams()
#
# self.states = C.reshape(self._dvs[:numXVars], [self.nStates(), self.nSteps])
# self.actions = C.reshape(self._dvs[numXVars:numXVars+numUVars], [self.nActions(), self.nSteps])
# self.params = self._dvs[numXVars+numUVars:]
#
# assert self.states.size() == numXVars
# assert self.actions.size() == numUVars
# assert self.params.size() == numPVars
# set up interface
self._constraints = Constraints()
self._bounds = Bounds(self.dae._xNames, self.dae._uNames, self.dae._pNames, self.nSteps)
self._initialGuess = InitialGuess(self.dae._xNames, self.dae._uNames, self.dae._pNames, self.nSteps)
self._designVars = DesignVars((self.dae._xNames,self.states),
(self.dae._uNames,self.actions),
(self.dae._pNames,self.params),
self.nSteps)
def nStates(self):
return self.dae.xVec().size()
def nActions(self):
return self.dae.uVec().size()
def nParams(self):
return self.dae.pVec().size()
def setIdasIntegrator(self, integratorOptions=[]):
# make dae input fun
daeSXFun = self.dae.sxFun()
assert self.nStates()==daeSXFun.inputSX(C.DAE_X).size()
assert self.nActions()+self.nParams()==daeSXFun.inputSX(C.DAE_P).size()
# make integrator
self.integrator = C.IdasIntegrator(daeSXFun)
setFXOptions(self.integrator, integratorOptions)
self.integrator.init()
# set up dynamics constraints
for k in range(0,self.nSteps-1):
uk = self.actions[:,k]
ukp1 = self.actions[:,k+1]
xk = self.states[:,k]
xkp1 = self.states[:,k+1]
p = self.params
upk = C.veccat([uk,p])
self.addConstraint(self.integrator.call([xk,upk])[C.INTEGRATOR_XF],'==',xkp1)
# constraints
def addConstraint(self,lhs,comparison,rhs,tag='unnamed_constraint'):
if hasattr(self, '_solver'):
raise ValueError("Can't add a constraint once the solver has been set")
self._constraints.add(lhs,comparison,rhs,tag)
# bounds
def setBound(self,name,val,**kwargs):
self._bounds.setBound(name,val,**kwargs)
# initial guess
def setGuess(self,name,val,**kwargs):
self._initialGuess.setGuess(name,val,**kwargs)
def setXGuess(self,*args,**kwargs):
self._initialGuess.setXVec(*args,**kwargs)
def setUGuess(self,*args,**kwargs):
self._initialGuess.setUVec(*args,**kwargs)
def getDesignVars(self):
return C.veccat( [C.flatten(self.states), C.flatten(self.actions), C.flatten(self.params)] )
#return self._dvs
# design vars
def lookup(self,name,timestep=None):
return self._designVars.lookup(name,timestep)
def devectorize(self,xup):
return self._designVars.devectorize(xup)
def getTimestepsFromDvs(self,dvs):
return self._designVars.getTimestepsFromDvs(dvs)
# solver
def setObjective(self, objective):
if hasattr(self, '_objective'):
raise ValueError("You've already set an objective and you can't change it")
self._objective = objective
def setSolver(self, solver, solverOptions=[], objFunOptions=[], constraintFunOptions=[]):
if hasattr(self, '_solver'):
raise ValueError("You've already set a solver and you can't change it")
if not hasattr(self, '_objective'):
raise ValueError("You need to set an objective")
# make objective function
f = C.MXFunction([self.getDesignVars()], [self._objective])
setFXOptions(f, objFunOptions)
f.init()
# make constraint function
g = C.MXFunction([self.getDesignVars()], [self._constraints.getG()])
setFXOptions(g, constraintFunOptions)
g.init()
def mkParallelG():
gs = [C.MXFunction([self.getDesignVars()],[gg]) for gg in self._constraints._g]
for gg in gs:
gg.init()
pg = C.Parallelizer(gs)
# pg.setOption("parallelization","openmp")
pg.setOption("parallelization","serial")
# pg.setOption("parallelization","expand")
pg.init()
dvsDummy = C.msym('dvs',(self.nStates()+self.nActions())*self.nSteps+self.nParams())
g_ = C.MXFunction([dvsDummy],[C.veccat(pg.call([dvsDummy]*len(gs)))])
g_.init()
return g_
# parallelG = mkParallelG()
# guess = self._initialGuess.vectorize()
# parallelG.setInput([x*1.1 for x in guess])
# g.setInput([x*1.1 for x in guess])
#
# g.evaluate()
# parallelG.evaluate()
# print parallelG.output()-g.output()
# exit(0)
# make solver function
# self._solver = solver(f, mkParallelG())
self._solver = solver(f, g)
setFXOptions(self._solver, solverOptions)
self._solver.init()
# set constraints
self._solver.setInput(self._constraints.getLb(), C.NLP_LBG)
self._solver.setInput(self._constraints.getUb(), C.NLP_UBG)
self.setBounds()
def setBounds(self):
lb,ub = self._bounds.get()
self._solver.setInput(lb, C.NLP_LBX)
self._solver.setInput(ub, C.NLP_UBX)
def solve(self):
self._solver.setInput(self._initialGuess.vectorize(), C.NLP_X_INIT)
self._solver.solve()
return self._solver.output(C.NLP_X_OPT)
class MultipleShootingStage():
def __init__(self, dae, nSteps):
# check inputs
assert isinstance(dae, Dae)
assert isinstance(nSteps, int)
# make sure dae has everything
assert hasattr(dae, '_residual')
self.dae = dae
self.dae._freeze('MultipleShootingStage(dae)')
# set up design vars
self.nSteps = nSteps
self.states = C.msym("x", self.nStates(), self.nSteps)
self.actions = C.msym("u", self.nActions(), self.nSteps)
self.params = C.msym("p", self.nParams())
# self._dvs = C.msym("dv",self.nStates()*self.nSteps+self.nActions()*self.nSteps+self.nParams())
#
# numXVars = self.nStates()*self.nSteps
# numUVars = self.nActions()*self.nSteps
# numPVars = self.nParams()
#
# self.states = C.reshape(self._dvs[:numXVars], [self.nStates(), self.nSteps])
# self.actions = C.reshape(self._dvs[numXVars:numXVars+numUVars], [self.nActions(), self.nSteps])
# self.params = self._dvs[numXVars+numUVars:]
#
# assert self.states.size() == numXVars
# assert self.actions.size() == numUVars
# assert self.params.size() == numPVars
# set up interface
self._constraints = Constraints()
self._bounds = Bounds(self.dae._xNames, self.dae._uNames,
self.dae._pNames, self.nSteps)
self._initialGuess = InitialGuess(self.dae._xNames, self.dae._uNames,
self.dae._pNames, self.nSteps)
self._designVars = DesignVars(
(self.dae._xNames, self.states), (self.dae._uNames, self.actions),
(self.dae._pNames, self.params), self.nSteps)
def nStates(self):
return self.dae.xVec().size()
def nActions(self):
return self.dae.uVec().size()
def nParams(self):
return self.dae.pVec().size()
def setIdasIntegrator(self, integratorOptions=[]):
# make dae input fun
daeSXFun = self.dae.sxFun()
assert self.nStates() == daeSXFun.inputSX(C.DAE_X).size()
assert self.nActions() + self.nParams() == daeSXFun.inputSX(
C.DAE_P).size()
# make integrator
self.integrator = C.IdasIntegrator(daeSXFun)
setFXOptions(self.integrator, integratorOptions)
self.integrator.init()
# set up dynamics constraints
for k in range(0, self.nSteps - 1):
uk = self.actions[:, k]
ukp1 = self.actions[:, k + 1]
xk = self.states[:, k]
xkp1 = self.states[:, k + 1]
p = self.params
upk = C.veccat([uk, p])
self.addConstraint(
self.integrator.call([xk, upk])[C.INTEGRATOR_XF], '==', xkp1)
# constraints
def addConstraint(self, lhs, comparison, rhs, tag='unnamed_constraint'):
if hasattr(self, '_solver'):
raise ValueError(
"Can't add a constraint once the solver has been set")
self._constraints.add(lhs, comparison, rhs, tag)
# bounds
def setBound(self, name, val, **kwargs):
self._bounds.setBound(name, val, **kwargs)
# initial guess
def setGuess(self, name, val, **kwargs):
self._initialGuess.setGuess(name, val, **kwargs)
def setXGuess(self, *args, **kwargs):
self._initialGuess.setXVec(*args, **kwargs)
def setUGuess(self, *args, **kwargs):
self._initialGuess.setUVec(*args, **kwargs)
def getDesignVars(self):
return C.veccat([
C.flatten(self.states),
C.flatten(self.actions),
C.flatten(self.params)
])
#return self._dvs
# design vars
def lookup(self, name, timestep=None):
return self._designVars.lookup(name, timestep)
def devectorize(self, xup):
return self._designVars.devectorize(xup)
def getTimestepsFromDvs(self, dvs):
return self._designVars.getTimestepsFromDvs(dvs)
# solver
def setObjective(self, objective):
if hasattr(self, '_objective'):
raise ValueError(
"You've already set an objective and you can't change it")
self._objective = objective
def setSolver(self,
solver,
solverOptions=[],
objFunOptions=[],
constraintFunOptions=[]):
if hasattr(self, '_solver'):
raise ValueError(
"You've already set a solver and you can't change it")
if not hasattr(self, '_objective'):
raise ValueError("You need to set an objective")
# make objective function
f = C.MXFunction([self.getDesignVars()], [self._objective])
setFXOptions(f, objFunOptions)
f.init()
# make constraint function
g = C.MXFunction([self.getDesignVars()], [self._constraints.getG()])
setFXOptions(g, constraintFunOptions)
g.init()
def mkParallelG():
gs = [
C.MXFunction([self.getDesignVars()], [gg])
for gg in self._constraints._g
]
for gg in gs:
gg.init()
pg = C.Parallelizer(gs)
# pg.setOption("parallelization","openmp")
pg.setOption("parallelization", "serial")
# pg.setOption("parallelization","expand")
pg.init()
dvsDummy = C.msym(
'dvs', (self.nStates() + self.nActions()) * self.nSteps +
self.nParams())
g_ = C.MXFunction([dvsDummy],
[C.veccat(pg.call([dvsDummy] * len(gs)))])
g_.init()
return g_
# parallelG = mkParallelG()
# guess = self._initialGuess.vectorize()
# parallelG.setInput([x*1.1 for x in guess])
# g.setInput([x*1.1 for x in guess])
#
# g.evaluate()
# parallelG.evaluate()
# print parallelG.output()-g.output()
# exit(0)
# make solver function
# self._solver = solver(f, mkParallelG())
self._solver = solver(f, g)
setFXOptions(self._solver, solverOptions)
self._solver.init()
# set constraints
self._solver.setInput(self._constraints.getLb(), C.NLP_LBG)
self._solver.setInput(self._constraints.getUb(), C.NLP_UBG)
self.setBounds()
def setBounds(self):
lb, ub = self._bounds.get()
self._solver.setInput(lb, C.NLP_LBX)
self._solver.setInput(ub, C.NLP_UBX)
def solve(self):
self._solver.setInput(self._initialGuess.vectorize(), C.NLP_X_INIT)
self._solver.solve()
return self._solver.output(C.NLP_X_OPT)
class Coll():
collocationIsSetup = False
def __init__(self, dae, nk=None, nicp=1, deg=4, collPoly='RADAU'):
assert(nk is not None)
assert(isinstance(dae, Dae))
self.dae = dae
self.nk = nk
self.nicp = nicp
self.deg = deg
self.collPoly = collPoly
self._xBounds = dict( [(name,[None]*(nk+1)) for name in dae.xNames()] )
self._zBounds = dict( [(name,[None]*nk) for name in dae.zNames()] )
self._uBounds = dict( [(name,[None]*nk) for name in dae.uNames()] )
self._pBounds = dict( [(name,None) for name in dae.pNames()] )
self._xGuess = dict( [(name,[None]*(nk+1)) for name in dae.xNames()] )
self._zGuess = dict( [(name,[None]*nk) for name in dae.zNames()] )
self._uGuess = dict( [(name,[None]*nk) for name in dae.uNames()] )
self._pGuess = dict( [(name,None) for name in dae.pNames()] )
self._constraints = Constraints()
# set (V,XD,XA,U,P)
self._setupDesignVars()
def setupCollocation(self,tf):
if self.collocationIsSetup:
raise ValueError("you can't setup collocation twice")
self.collocationIsSetup = True
## -----------------------------------------------------------------------------
## Collocation setup
## -----------------------------------------------------------------------------
# Size of the finite elements
self.h = tf/self.nk/self.nicp
# make coefficients for collocation/continuity equations
C,D,tau,tau_root = setupCoeffs(deg=self.deg,collPoly=self.collPoly,nk=self.nk,h=self.h)
self.tau_root = tau_root
self.tau = tau
ffcn = self.makeResidualFun()
# function to get h out
self.hfun = CS.MXFunction([self._V],[self.h])
self.hfun.init()
# add collocaiton constraints
(g_coll,lbg_coll,ubg_coll) = setupCollocationConstraints(self,C,ffcn,self.h,D)
assert(len(g_coll)==len(lbg_coll) and len(g_coll)==len(ubg_coll))
for k in range(len(g_coll)):
assert(lbg_coll[k].size == ubg_coll[k].size)
if lbg_coll[k].size>0:
assert(g_coll[k].size()==lbg_coll[k].size)
self.constrainBnds(g_coll[k],(lbg_coll[k],ubg_coll[k]))
def xVec(self,timestep=None):
if not isinstance(timestep, int):
raise ValueError("timestep needs to be an int")
return self._XD[timestep][0][0]
def uVec(self,timestep=None):
if not isinstance(timestep, int):
raise ValueError("timestep needs to be an int")
return self._U[timestep]
def pVec(self):
return self._P
def zVec(self,timestep=None):
raise ValueError("zVec not yet safe to use cause it's not defined at the beginning of the interval, need to implement moar inputs to zVec")
if not isinstance(timestep, int):
raise ValueError("timestep needs to be an int")
if timestep==0:
raise ValueError("there is no algebraic state at timestep 0")
assert(timestep>0)
return self._XA[timestep-1][-1][-1]
def constrain(self,lhs,comparison,rhs):
self._constraints.add(lhs,comparison,rhs)
def constrainBnds(self,g,(lbg,ubg)):
self._constraints.addBnds(g,(lbg,ubg))
