def __init__(self,
ocp,
lqrDae,
ocpOptions=None,
integratorOptions=None,
codegenOptions=None,
phase1Options=None):
assert isinstance(
ocp, Mpc), "MpcRT must be given an Mpc object, you gave: " + str(
type(ocp))
# call the parent init
OcpRT.__init__(self,
ocp,
ocpOptions=ocpOptions,
integratorOptions=integratorOptions,
codegenOptions=codegenOptions,
phase1Options=phase1Options)
# set up measurement functions
self._yxFun = C.SXFunction([ocp.dae.xVec()], [C.densify(self.ocp.yx)])
self._yuFun = C.SXFunction([ocp.dae.uVec()], [C.densify(self.ocp.yu)])
self._yxFun.init()
self._yuFun.init()
self._lqrDae = lqrDae
self._integratorLQR = rawe.RtIntegrator(self._lqrDae,
ts=self.ocp.ts,
options=integratorOptions)
def __init__(self,
ocp,
ocpOptions=None,
integratorOptions=None,
codegenOptions=None,
phase1Options=None,
integratorMeasurements=None):
if ocpOptions is None:
ocpOptions = OcpExportOptions(),
if integratorOptions is None:
integratorOptions = RtIntegratorOptions(),
if codegenOptions is None:
codegenOptions = {}
if phase1Options is None:
phase1Options = {}
assert isinstance(
ocp, Ocp), "OcpRT must be given an Ocp object, you gave: " + str(
type(ocp))
self._ocp = ocp
exportPath = self.ocp.exportCode(ocpOptions, integratorOptions,
codegenOptions, phase1Options)
self._exportPath = exportPath
self._libpath = os.path.join(self._exportPath, 'ocp.so')
self._lib = ctypes.cdll.LoadLibrary(self._libpath)
# set return types of KKT,objective,etc
self._lib.getKKT.restype = ctypes.c_double
self._lib.getObjective.restype = ctypes.c_double
self._lib.preparationStepTimed.restype = ctypes.c_double
self._lib.feedbackStepTimed.restype = ctypes.c_double
self.preparationTime = 0.0
self.feedbackTime = 0.0
self.x = numpy.zeros(
(self._lib.py_get_ACADO_N() + 1, self._lib.py_get_ACADO_NX()))
self.u = numpy.zeros(
(self._lib.py_get_ACADO_N(), self._lib.py_get_ACADO_NU()))
self.y = numpy.zeros(
(self._lib.py_get_ACADO_N(), self._lib.py_get_ACADO_NY()))
if self._lib.py_get_ACADO_NXA() > 0:
self.z = numpy.zeros(
(self._lib.py_get_ACADO_N(), self._lib.py_get_ACADO_NXA()))
self.yN = numpy.zeros(self._lib.py_get_ACADO_NYN())
wmt = self._lib.py_get_ACADO_WEIGHTING_MATRICES_TYPE()
if wmt == 1:
self.S = numpy.zeros(
(self._lib.py_get_ACADO_NY(), self._lib.py_get_ACADO_NY()))
self.SN = numpy.zeros(
(self._lib.py_get_ACADO_NYN(), self._lib.py_get_ACADO_NYN()))
elif wmt == 2:
self.S = numpy.zeros(
(self._lib.py_get_ACADO_N() * self._lib.py_get_ACADO_NY(),
self._lib.py_get_ACADO_NY()))
self.SN = numpy.zeros(
(self._lib.py_get_ACADO_NYN(), self._lib.py_get_ACADO_NYN()))
else:
raise Exception('unrecognized ACADO_WEIGHING_MATRICES_TYPE ' +
str(wmt))
if self._lib.py_get_ACADO_INITIAL_STATE_FIXED():
self.x0 = numpy.zeros(self._lib.py_get_ACADO_NX())
print 'initializing solver'
self._lib.py_initialize()
self._getAll()
self._log = {}
self._autologNames = []
for field in self._canonicalNames:
if hasattr(self, field):
self._autologNames.append(field)
self._log[field] = []
self._log['_kkt'] = []
self._log['_objective'] = []
self._log['_prep_time'] = []
self._log['_fb_time'] = []
self._log['outputs'] = {}
for outName in self.outputNames():
self._log['outputs'][outName] = []
# setup outputs function
self._outputsFun = self.ocp.dae.outputsFunWithSolve()
# export integrator
self._integrator = rawe.RtIntegrator(
self.ocp.dae,
ts=self.ocp.ts,
options=integratorOptions,
measurements=integratorMeasurements)
self._integratorOptions = integratorOptions
cgOpts = {'CXX': 'g++', 'CC': 'gcc'}
# cgOpts = {'CXX':'icpc', 'CC':'icc'}
return rawe.OcpRT(mhe, codegenOptions=cgOpts)
if __name__ == '__main__':
N = 70
ts = 0.2
A = 0.5
omega = 0.3
dae = makeDae()
mpc = makeMpc(dae, N, ts)
sim = rawe.RtIntegrator(dae, ts=ts)
# sim = rawe.sim.Sim(dae, ts=ts)
print '=' * 80
# set the mpc weights
xRms = 0.1
vRms = 1.5
fRms = 5.0
mpc.S[0, 0] = (1.0 / xRms)**2 / N
mpc.S[1, 1] = (1.0 / vRms)**2 / N
mpc.S[2, 2] = (1.0 / fRms)**2 / N
mpc.SN[0, 0] = (1.0 / 0.01)**2
mpc.SN[1, 1] = (1.0 / 0.01)**2
# initial guess
dae.setResidual([dae.ddt("x") - v, dae.ddt("v") - u])
# Simulation parameters
N_mpc = 10 # Number of MPC control intervals
N_mhe = 10 # Number of MHE control intervals
Ts = 0.1 # Sampling time
Tf = 10. # Simulation duration
# Create the MPC class
mpcRT = makeNmpc(dae, N=N_mpc, dt=Ts)
mheRT = makeMhe(dae, N=N_mpc, dt=Ts)
InitializeMPC(mpcRT, dae)
InitializeMHE(mheRT, dae)
Rint = rawe.RtIntegrator(dae, ts=Ts, options=intOpts)
#Rint.getOutputs()
# Initialize the MPC-MHE scheme
#mpcRT.initialize()
#mheRT.initialize()
# Create a simulation class
#intOptions = {'type':'Rintegrator', 'numIntegratorSteps':400, 'integratorType':'INT_IRK_GL2', 'ts':Ts}
intOptions = {'type': 'Idas', 'ts': Ts}
sim, simLog = InitializeSim(dae, intOptions)
mpcRT.x0 = np.array([1, 0])
outs = sim.getOutputs(mpcRT.x[0, :], mpcRT.u[0, :], {})
new_y = np.squeeze(outs['measurements'])
simLog.log(mpcRT.x0, new_y, [])