diff --git a/.gitignore b/.gitignore index 034841a..d9005f2 100644 --- a/.gitignore +++ b/.gitignore @@ -26,14 +26,6 @@ share/python-wheels/ *.egg MANIFEST -# Project Files -notes.py -*.gif -*.jpg -.vscode -.vscode/* -*.json - # PyInstaller # Usually these files are written by a python script from a template # before PyInstaller builds the exe, so as to inject date/other infos into it. diff --git a/Interface/controller.py b/Interface/controller.py deleted file mode 100644 index e69de29..0000000 diff --git a/Interface/gui.py b/Interface/gui.py deleted file mode 100644 index e69de29..0000000 diff --git a/MPC/__init__.py b/MPC/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/MPC/model.py b/MPC/model.py deleted file mode 100644 index 7b5362c..0000000 --- a/MPC/model.py +++ /dev/null @@ -1,214 +0,0 @@ -from typing import Any -import numpy as np -import do_mpc -from casadi import * -import json - -# TODO: make assert statements on attributes -class Model: - """ - Setup MPC model:\n - Number of Agents\n - System matrices: A, B, C, D\n - Matrices sizes:dx, du, dy\n - """ - # Model Matrices - nb_agents = 1 - - # model_type = 'discrete' # either 'discrete' or 'continuous' - # model = do_mpc.model.Model(model_type) - - # An = np.zeros(4) - # Bn = np.zeros((4,2)) - # Cn = np.zeros((2,4)) - # Dn = np.zeros(2) - - __A = np.zeros(4) - __B = np.zeros((4,2)) - __C = np.zeros((2,4)) - __D = np.zeros(2) - - # Sizes - dx, du = np.shape(__B) - dy = np.size(__C, 0) - - # Optimizer variables - __var_x = [] - # __var_y = [] - __var_u = [] - __var_target = [] - - X = None - Y = None - U = None - Target = None - - - def __init__(self): - model_type = 'discrete' # either 'discrete' or 'continuous' - self.model = do_mpc.model.Model(model_type) - - # def __setattr__(self, __name: str, __value: Any) -> None: - # if __name in ('X', 'Y', 'U', 'Target', '__var_x', '__var_y', '__var_u', '__var_target', '__A', '__B', '__C', '__D'): - # raise AttributeError("'{}' is read-only. Use dedicated functions to set __A new value".format(__name)) - - def setupModel(self): - # Set the inputs, states and parameters for all agents - res1 = self.setModelVariables() - - # Set x_next, y_next symbolic expressions - res2 = self.setModelExpressions() - - # Set right hand equation for all agents - res3 = self.setModelRhs() - - try: - self.model.setup() - except: - return "MPC.model.setupModel(): Error setting up model\nMPC.model.setAgentStruct(): "+str(res1)+ "\nMPC.model.setModelExpressions()"+ str(res2)+"\nMPC.model.setModelRhs(): "+str(res3) - - return 0 - - def setModelVariables(self): - - try: - for i in range(self.nb_agents): - # Set symbolic name for variables - x_name = 'x' + str(i) - u_name = 'u' + str(i) - # y_name = 'y' + str(i) - target_name = 'target' + str(i) - - # States struct (optimization variables): - self.__var_x.append(self.model.set_variable(var_type='_x', var_name=x_name, shape=(self.dx,1))) - # self.__var_y.append(self.model.set_variable(var_type='_x', var_name=y_name, shape=(self.dy,1))) - # Input struct (optimization variables): - self.__var_u.append(self.model.set_variable(var_type='_u', var_name=u_name, shape=(self.du,1))) - # parameters - self.__var_target.append(self.model.set_variable(var_type='_tvp', var_name=target_name, shape=(self.dx,1))) - - except: - return "MPC.optimizerModel.setModelVariables(): Error setting optimizer agents states, inputs and parameters structure" - - return 0 - - def setGlobalModelVariables(self): - """setGlobalModelVariables(self) --> [X, Y, U, Target]""" - try: - # isNotSingleton = len(self.__var_x) > 1 and len(self.__var_y) > 1 and len(self.__var_u) > 1 and len(self.__var_target) > 1 - isNotSingleton = len(self.__var_x) > 1 and len(self.__var_u) > 1 and len(self.__var_target) > 1 - if isNotSingleton: - # self.X, self.Y, self.U, self.Target = [horzcat(*self.__var_x), horzcat(*self.__var_y), horzcat(*self.__var_u), horzcat(*self.__var_target)] - self.X, self.U, self.Target = [horzcat(*self.__var_x), horzcat(*self.__var_u), horzcat(*self.__var_target)] - else: - # self.X, self.Y, self.U, self.Target = [self.__var_x[0], self.__var_y[0], self.__var_u[0], self.__var_target[0]] - self.X, self.U, self.Target = [self.__var_x[0], self.__var_u[0], self.__var_target[0]] - except: - # return "MPC.optimizerModel.setGlobalModelVariables(): Error setting modelVariables, check __var_x, __var_y, __var_u and __var_target" - return "MPC.optimizerModel.setGlobalModelVariables(): Error setting modelVariables, check __var_x, __var_u and __var_target" - return 0 - - def setModelExpressions(self): - # res1 = self.setGlobalSystem() - res2 = self.setGlobalModelVariables() - - try: - self.model.set_expression('X_next', self.__A@self.X + self.__B@self.U) - self.model.set_expression('Y_next', self.__C@self.X + self.__D@self.U) - - except: - return "MPC.optimizerModel.setModelExpressions(): Error setting expressions\nMPC.optimizerModel.setGlobalSystem(): "+str(res2) - - return 0 - - def setModelRhs(self): - try: - for i in range(self.nb_agents): - self.model.set_rhs("x"+str(i), self.__A@self.model.x["x"+str(i)] + self.__B@self.model.u["u"+str(i)]) - self.model.set_rhs("y"+str(i), self.__C@self.model.x["x"+str(i)] + self.__D@self.model.u["u"+str(i)]) - - except: - return "MPC.optimizerModel.setModelRhs(): Error setting model rhs" - - return 0 - - def setAgentSystem(self, filepath:str=None, matrixList:list[np.array]=None): - """ setSystem(self, file=None, matrixList=None) - --> 0 : ok - --> msg : error in processing - - - Example of file: - { - '__A':[ - [1, 1, 1, 1], - [1, 1, 1, 1], - [1, 1, 1, 1], - [1, 1, 1, 1], - ], - - '__B':[ - [0, 0], - [0, 0], - [0.5, 0], - [0, 0.5] - ] - - (...) - } - - - Ecample of matrix list: - list = [np.array(4), np.array(4), .., ..] - - """ - if filepath is not None and matrixList is None: - try: - # Get json file - file = open(filepath) - data = json.load(file) - - # Construct System matrice - self.__A = np.array(data['A']) - self.__B = np.array(data['B']) - self.__C = np.array(data['C']) - self.__D = np.array(data['D']) - - # update size parameter - self.dx, self.du = np.shape(self.__B) - self.dy = np.size(self.__C, 0) - - except: - return "MPC.optimizerModel.setSystem(): Error in file input processing" - - - elif filepath is None and matrixList is not None: - try: - self.__A, self.__B, self.__C, self.__D = matrixList - # update size parameter - self.dx, self.du = np.shape(self.__B) - self.dy = np.size(self.__C, 0) - except: - return "MPC.optimizerModel.setModel(): Error in matrix list input processing" - else: - return "MPC.optimizerModel.setSystem(): Error cannot have input file and input matrix list together" - - return 0 - - # def setGlobalSystem(self): - # try: - # # set mpc System according to number of agents - # self.An = np.concatenate(tuple([self.__A for i in range(self.nb_agents )]), axis=0) - # self.Bn = np.concatenate(tuple([self.__B for i in range(self.nb_agents )]), axis=0) - # self.Cn = np.concatenate(tuple([self.__C for i in range(self.nb_agents )]), axis=0) - # self.Dn = np.concatenate(tuple([self.__D for i in range(self.nb_agents )]), axis=0) - # except: - # return "MPC.optimizerModel.setGlobalSystem(): Error setting global matrices" - # return 0 - - - def getAgentSystem(self): - """getModel(self) --> [self.__A, self.__B, self.__C, self.__D ]""" - return [self.__A, self.__B, self.__C, self.__D ] - - def getSizes(self): - """getSizes(self) --> [self.dx, self.du, self.dy]""" - return [self.dx, self.du, self.dy] \ No newline at end of file diff --git a/MPC/optimizer.py b/MPC/optimizer.py deleted file mode 100644 index 3020a8a..0000000 --- a/MPC/optimizer.py +++ /dev/null @@ -1,200 +0,0 @@ -import numpy as np -import do_mpc -from casadi import * -from MPC.model import Model -import pypoman -import polytope - -# TODO: make assert statements on attributes -class Optimizer: - # Simulation time - Nsim = 100 - - __setup_mpc = { - 'n_horizon': 5, - 't_step': 1, # TODO: set position vector for every step - 'n_robust': 1, - 'store_full_solution': True, - } - - # Constraints/Parameters - umin = None - umax = None - xmin = None - xmax = None - - # first state - # xinit = None - - # Obstacle - listOfObstacle = [] - - # Trajectory - listOfPositions = [] # position = tuple (time, array) - - - def __init__(self, model:Model): - - try: - self.model = model - - # set up mpc - self.mpc = do_mpc.controller.MPC(self.model.model) - - # Set parameters - self.mpc.set_param(**self.__setup_mpc) - - # Set tuning matrices - self.Q = np.eye(model.dx) - self.P = 10*self.Q - self.R = 10 - - # default initial and target position - # self.xinit = np.ones((self.model.dx, self.model.nb_agents)) - self.listOfPositions.append((0, np.zeros((self.model.dx, self.model.nb_agents)))) - - except: - e = sys.exc_info()[0] - raise Exception("MPC.optimizer.__init__():\n"+e) - - def setup(self): - try: - self.mpc.setup() - except: - return "MPC.optimizer.setup(): Error setting up optimizer. Did you run all the steps correctly?" - - return 0 - - def setObstacleConstraints(self): - - try: - for i in range(len(self.listOfObstacle)): - poly = self.listOfObstacle[i] - center = self.__Centroid__(pypoman.compute_polytope_vertices(poly.A, poly.b)) - print(center) - for j in range(self.model.nb_agents): - x = self.model.model.x["x"+str(j)] - limit = -abs(np.amax(poly.b)) - (1*abs(np.amax(poly.b)))/100 # A marge of max(radius)+5% - self.mpc.set_nl_cons('obs_'+str(i)+'_constr_'+str(j), -sumsqr((x[:self.model.dy]-SX(center))**2), ub=limit) - - except: - return "MPC.optimizer.setObstacleConstraints(): Error setting obstacle constraints. Check polyhedrons or model" - - return 0 - - def setBoundsConstraints(self): - try: - - for i in range(self.model.nb_agents): - - self.mpc.bounds['lower','_u', "u"+str(i)] = self.umin - self.mpc.bounds['upper','_u', "u"+str(i)] = self.umax - self.mpc.bounds['lower','_x', "x"+str(i)] = self.xmin - self.mpc.bounds['upper','_x', "x"+str(i)] = self.xmax - - except: - e = str(sys.exc_info()[0]) + ": " + str(sys.exc_info()[1]) - raise Exception("MPC.optimizer.setBoundsConstraints():\n"+ str(e)) - # return "MPC.optimizer.setBoundsConstraints(): Error setting obstacle constraints. Check polyhedrons or model" - return 0 - - def setTrajectory(self): - tvp_template_mpc = self.mpc.get_tvp_template() - Npred = self.__setup_mpc['n_horizon'] - positions = self.createPositionsVector() - - # Set function which returns time-varying parameters - def tvp_fun_mpc(t_now): - for i in range(self.model.nb_agents): - for k in range(Npred): - tvp_template_mpc['_tvp', k, 'target'+str(i)] = positions[int(t_now+k)] - - return tvp_template_mpc - self.mpc.set_tvp_fun(tvp_fun_mpc) - - # try: - # tvp_template_mpc = self.mpc.get_tvp_template() - # Npred = self.__setup_mpc['n_horizon'] - # positions = self.createPositionsVector() - - # # Set function which returns time-varying parameters - # def tvp_fun_mpc(t_now): - # for i in range(self.model.nb_agents): - # for k in range(Npred): - # tvp_template_mpc['_tvp', k, 'target'+str(i)] = positions[int(t_now+k)] - - # return tvp_template_mpc - # self.mpc.set_tvp_fun(tvp_fun_mpc) - - # except: - # return "MPC.optimizer.setTrajectory(): Error setting up trajectory check positions vectors" - - return 0 - - def __Centroid__(self, vertexes:list): - try: - _x_list = [vertex [0] for vertex in vertexes] - _y_list = [vertex [1] for vertex in vertexes] - _len = len(vertexes) - _x = sum(_x_list) / _len - _y = sum(_y_list) / _len - - except: - return "MPC.optimizer.__Centroid__(): Error finding center of polyhedrons." - - return [_x, _y] - - def createPositionsVector(self) -> list: - try: - current_t = 0 - next_t = self.listOfPositions[0][0] - lentgh = len(self.listOfPositions) - positions = [] - - for i in range(lentgh): - positions += [self.listOfPositions[i][1] for x in range(current_t,next_t)] - - if i == lentgh-1: - next_t = self.__setup_mpc['n_horizon'] + self.Nsim - else: - next_t = self.listOfPositions[i+1][0] - - current_t = self.listOfPositions[i][0] - - positions += [self.listOfPositions[i][1] for x in range(current_t,next_t)] - - except: - return "MPC.optimizer.createPositionsVector(): Error creating position vector for trajectory. Check ListOfPositions attribute is set correctly" - - return positions - - def setObjective(self): - - try: - mterm = 0 - lterm = 0 - rterm = {} - for i in range(self.model.nb_agents): - mterm += mtimes(mtimes((self.model.model.x["x"+str(i)]-self.model.model.tvp["target"+str(i)]).T,self.Q), (self.model.model.x["x"+str(i)]-self.model.model.tvp["target"+str(i)])) - lterm += mtimes(mtimes((self.model.model.x["x"+str(i)]-self.model.model.tvp["target"+str(i)]).T,self.P), (self.model.model.x["x"+str(i)]-self.model.model.tvp["target"+str(i)])) - rterm["u"+str(i)] = self.R - - self.mpc.set_objective(mterm=mterm, lterm=lterm) - self.mpc.set_rterm( - **rterm - ) - - - except: - return "MPC.optimizer.setObjective(): Error setting Objective. Check model states, target or tuning matrices" - - return 0 - self.M = nb - - def getTuningMatrices(self): - """getTuningMatrices(self) --> [self.Q, self.P, self.R]""" - return [self.Q, self.P, self.R] - - def setHorizon(self, nb:int): - self.__setup_mpc['n_horizon'] = nb - self.mpc.set_param(**self.__setup_mpc) \ No newline at end of file diff --git a/MPC/simulator.py b/MPC/simulator.py deleted file mode 100644 index aabafaa..0000000 --- a/MPC/simulator.py +++ /dev/null @@ -1,173 +0,0 @@ -import json -from MPC.optimizer import Optimizer -import numpy as np -import do_mpc -from casadi import * -import matplotlib.pyplot as plt -import matplotlib as mpl -from matplotlib.animation import FuncAnimation, ImageMagickWriter, FFMpegFileWriter, FileMovieWriter -from matplotlib.patches import Polygon -import pypoman - -# TODO: make assert statements on attributes -# TODO: function to update,add,delete figure config in sim_struct.json: add "figname":{info} to file setFig(*args). If fig name already exists update else create new entry - -class Simulator: - - __location__ = os.path.realpath( - os.path.join(os.getcwd(), os.path.dirname(__file__)) - ) - - def __init__(self, optimizer: Optimizer) -> None: - try: - self.optimizer = optimizer - - # Set up simulator - self.simulator = do_mpc.simulator.Simulator(optimizer.model.model) - - # Instead of supplying a dict with the splat operator (**), as with the optimizer.set_param(), - # we can also use keywords (and call the method multiple times, if necessary): - self.simulator.set_param(t_step = 0.1) - - # Set function which returns time-varying parameters - tvp_template_sim = self.simulator.get_tvp_template() - def tvp_fun_sim(t_now): - return tvp_template_sim - self.simulator.set_tvp_fun(tvp_fun_sim) - - self.simulator.setup() - self.estimator = do_mpc.estimator.StateFeedback(optimizer.model.model) - - # First state - self.xinit = np.ones((optimizer.model.dx + optimizer.model.dy, optimizer.model.nb_agents)) - - - # Simulator results - self.usim = DM(np.zeros((optimizer.model.du, optimizer.Nsim))); - self.ysim = DM(np.zeros((optimizer.model.dy, optimizer.Nsim+1))); - self.xsim = DM(np.zeros((optimizer.model.dx, optimizer.Nsim+1))); - self.targetsim = DM(optimizer.createPositionsVector()) - - self.xsim[:,0] = self.xinit[:optimizer.model.dx] - - except: - e = str(sys.exc_info()[0]) + ": " + str(sys.exc_info()[1]) - raise Exception("MPC.simulator.py.__init__():\n"+ str(e)) - - def launchSimulation(self): - try: - # Initialize simulation - self.simulator.x0 = self.xinit - self.estimator.x0 = self.xinit - self.optimizer.mpc.x0 = self.xinit - - self.optimizer.mpc.set_initial_guess() - A, B, C, D = self.optimizer.model.getAgentSystem() - - x0 = self.xinit - - for k in range(self.optimizer.Nsim): - u0 = self.optimizer.mpc.make_step(x0) - y_next = self.simulator.make_step(u0) - x0 = self.estimator.make_step(y_next) - - self.usim[:,k] = u0 - self.xsim[:,k+1] = A@x0[:self.optimizer.model.dx] + B@u0[:self.optimizer.model.du] - except: - e = str(sys.exc_info()[0]) + ": " + str(sys.exc_info()[1]) - print("MPC.simulator.py.launchSimulation():\n"+ e) - - return 0 - - def show(self, _figname:str=None): - - try: - titles, data = self.getSimStruct() - - # To plot a particular figure in sim_struct else plot all figures - if _figname is not None: - data = {_figname:data[_figname]} - titles = [_figname] - - print(data) - # Initialize figures - nb_plot = len(data) - fig = [None]*nb_plot - ax = [None]*nb_plot - - - for i in range(nb_plot): - plt.ion() - fig[i] = plt.figure(i) - ax[i] = fig[i].add_subplot() - - config = data[str(titles[i])] - vars = config["variables"] - print("[info]: plotting ", titles[i]) - - fig[i].suptitle(titles[i]) - ax[i].set_xlabel(config['xlabel']) - ax[i].set_ylabel(config['ylabel']) - ax[i].set_xlim(config['xlim']) - ax[i].set_ylim(config['ylim']) - - # Set obstacle if there is - if config['has_obstacles']: - for obs in self.optimizer.listOfObstacle: - polygon = Polygon(pypoman.compute_polytope_vertices(obs.A, obs.b)) - ax[i].add_patch(polygon) - - plt.grid() - # Plot at each step - for k in range(self.optimizer.Nsim): - # plot each variable - for j in range(len(config["variables"])): - kwargs = {'color':config["colors"][j], 'marker':config["markers"][j], 'linestyle':config["linestyles"][j]} - args = [self.__toVar__(vars[j])[x,k] for x in config["indexes"][j]] # set the variables to plot - if len(args) == 1: - args = [k] + args - - ax[i].plot(*args, **kwargs) - fig[i].canvas.draw() - fig[i].canvas.flush_events() - - plt.show() - except: - e = str(sys.exc_info()[1]) - print("MPC.simulator.py.show():\n"+ str(e)) - - return 0 - - def getSimStruct(self): - titles = [] - try: - file = open(os.path.join(self.__location__, "sim_struct.json")) - data = json.load(file) - titles = list(data.keys()) - # Close opend file - file.close() - except: - e = str(sys.exc_info()[0]) + ": " + str(sys.exc_info()[1]) - raise Exception("MPC.simulator.py.__init__():\n"+ str(e)) - - return titles, data - - def __toVar__(self, varname:str): - try: - var = None - if varname == "xsim": - var = self.xsim - elif varname == "ysim": - var = self.ysim - elif varname == "usim": - var = self.usim - elif varname == "targetsim": - var = self.targetsim.T - else: - pass - - except: - e = str(sys.exc_info()[1]) - print("MPC.simulator.py.__toVar__():\n"+ str(e)) - - return var \ No newline at end of file diff --git a/README.md b/README.md index 77fd88e..2601447 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,2 @@ # MPC_Interface A graphical interface for testing a MPC controller - -# TODO -- Write custom exception class to be raised in all MPC modules \ No newline at end of file diff --git a/main.py b/main.py deleted file mode 100644 index 809d43b..0000000 --- a/main.py +++ /dev/null @@ -1,110 +0,0 @@ -import time -from MPC.model import Model -import numpy as np -from MPC.optimizer import Optimizer -import polytope as poly -import matplotlib.pyplot as plt -import matplotlib as mpl -from matplotlib.animation import FuncAnimation, ImageMagickWriter, FFMpegFileWriter, FileMovieWriter -import polytope as poly -from matplotlib.patches import Polygon -from MPC.simulator import Simulator -import pypoman - -model = Model() - -""" Set number of agents""" -model.nb_agents = 1 -# model.A = np.zeros(4) -# model.var_u = [] -# set single agent system -res = model.setAgentSystem(filepath="system.json") -print(res) -A, B, C, D = model.getAgentSystem() -print("A='{}'\nB='{}'\nC='{}'\nD='{}'\n".format(A, B, C, D) ) -# print(model.A) -# print(model.B) -# print(model.C) -# print(model.D) -# print(model.getSizes()) - -"""setup model""" -res = model.setupModel() -print(res) -# print(model.var_x) -# print(model.var_y) -# print(model.var_u) -# print(model.var_target) -print(model.model.x.keys()) - -# print(res) -# print(np.shape(model.X)) -# print(model.X) -# print(np.shape(model.An)) -# print(model.An) -print(np.shape(model.model.aux['X_next'])) -print(model.model.aux['X_next']) - -print(model.model) - -"""Setup Optimizer""" -optimizer = Optimizer(model) - -optimizer.setHorizon(20) - - -"""Set Objective""" -res = optimizer.setObjective() -print(res) - - -"""Set Obstacle Constraints""" -# Polyhedron definition -Ax = np.array([ [-1, 0], - [1, 0], - [0, -1], - [0, 1] - ]) - -bx = 0.4*np.ones((4,1)) -obs = poly.Polytope(Ax, bx) -optimizer.listOfObstacle.append(obs) -print(optimizer.listOfObstacle) - -# Set constraints -optimizer.umin = -2 -optimizer.umax = 2 -optimizer.xmin = -10 -optimizer.xmax = 10 - -res = optimizer.setObstacleConstraints() -res = optimizer.setBoundsConstraints() -# res = optimizer.set -print(res) - -# Set Trajectory -optimizer.listOfPositions = [(15,np.array([5, 5, 0, 0])), (30,np.array([-3, 0, 0, 0])), (50,np.array([4, 1, 0, 0])) ] -res = optimizer.setTrajectory() -print(res) - -optimizer.setup() - -"""Set up simulator""" -# optimizer.xinit = np.array([2,2,0,0,0,0]) -simulator = Simulator(optimizer) -simulator.xinit = np.array([2, 2, 0, 0]) -# simulate -res = simulator.launchSimulation() -# show plot -res = simulator.show("Position") -res = simulator.show("Vx(t)") -res = simulator.show("Vy(t)") - -# for k in range(optimizer.Nsim): -# print(simulator.xsim[:,k]) -# ax[0].scatter(simulator.xsim[0,k].full(), simulator.xsim[1,k].full(), color='blue') -# ax[1].stem(k, simulator.xsim[2,k].full(), '--', use_line_collection = True) -# ax[2].stem(k, simulator.xsim[3,k].full(), '--', use_line_collection = True) - -# fig.canvas.draw() -# fig.canvas.flush_events() diff --git a/requirements.txt b/requirements.txt deleted file mode 100644 index aa2d2c5..0000000 Binary files a/requirements.txt and /dev/null differ diff --git a/state_target_tracking_dompc b/state_target_tracking_dompc deleted file mode 100644 index 8e36cc9..0000000 --- a/state_target_tracking_dompc +++ /dev/null @@ -1,299 +0,0 @@ -import numpy as np -import do_mpc -from casadi import * -import matplotlib.pyplot as plt -import matplotlib as mpl -from matplotlib.animation import FuncAnimation, ImageMagickWriter, FFMpegFileWriter, FileMovieWriter -import polytope as poly -from matplotlib.patches import Polygon -import pypoman - -model_type = 'discrete' # either 'discrete' or 'continuous' -model = do_mpc.model.Model(model_type) - -# System -A = np.array([ [1, 0, 0.5, 0], - [0, 1, 0, 0.5], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) - -B = np.array([ [0, 0], - [0, 0], - [0.5, 0], - [0, 0.5] - ]) - -C = np.array([ [1, 0, 0, 0], - [0, 1, 0, 0] - ]) - -D = np.zeros((2,2)) - -# Obstacle -Ax = np.array([ [-1, 0], - [1, 0], - [0, -1], - [0, 1] - ]) - -bx = 0.4*np.ones((4,1)) -obs = poly.Polytope(Ax, bx) -M = 1000 -vertices = pypoman.compute_polytope_vertices(Ax, bx) -print(vertices[0][0]) -print(vertices) -# poly = polytope.qhull(vertices) - -# dimensions -dx, du = np.shape(B) -dy = np.size(C, 0) - -# Constraints -umin = -10 -umax = 10 -delta_u_min = -0.1 -delta_u_max = 0.1 -ymin = -10; -ymax = 10; - -#Tuning matrixes -Q = np.eye(4) -Qy = np.eye(dy) -P = Q*10 -R = 10 - -# parameters -x0 = np.array([2, 2, 0, 0, 1, 1]) -# target = np.array([5, 5, 0, 0]) - -# States struct (optimization variables): -x = model.set_variable(var_type='_x', var_name='x', shape=(dx,1)) -y = model.set_variable(var_type='_x', var_name='y', shape=(dy,1)) -# Input struct (optimization variables): -u = model.set_variable(var_type='_u', var_name='u', shape=(du,1)) -# alpha = model.set_variable(var_type='_u', var_name='alpha', shape=(np.size(obs.A,0),1)) -# parameters -target = model.set_variable(var_type='_tvp', var_name='target', shape=(dx,1)) - -# simulator param -Npred = 30; -Nsim = 80; - -x_next = A@x + B@u -y_next = C@x + D@u - -model.set_rhs('x', x_next) -model.set_rhs('y', y_next) - -model.setup() - -# set up mpc -mpc = do_mpc.controller.MPC(model) - -# optimizer parameter -setup_mpc = { - 'n_horizon': Npred, - 't_step': 1, - 'n_robust': 1, - 'store_full_solution': True, -} -mpc.set_param(**setup_mpc) -# print(mtimes(mtimes(x.T,Q), x)) -mterm = mtimes(mtimes((x-target).T,Q), (x-target)) -lterm = mtimes(mtimes((x-target).T,P), (x-target)) - -mpc.set_objective(mterm=mterm, lterm=lterm) - -mpc.set_rterm( - u = R -) - -# # Constraints -# # Lower bounds on states: -# mpc.bounds['lower','_x', 'x'] = -2*np.pi -# mpc.bounds['lower','_x', 'y'] = -2*np.pi -# # Upper bounds on states -# mpc.bounds['upper','_x', 'x'] = 2*np.pi -# mpc.bounds['upper','_x', 'y'] = 2*np.pi - -# Lower bounds on inputs: -# mpc.bounds['lower','_u', 'u'] = -2*np.pi -# mpc.bounds['lower','_u', 'alpha'] = -2*np.pi -# mpc.bounds['lower','_u', 'alpha'] = 0 -# Upper bounds on inputs: -# mpc.bounds['upper','_u', 'u'] = 2*np.pi -# mpc.bounds['upper','_u', 'alpha'] = 2 -# mpc.set_nl_cons('obs_constr_1', -mtimes(obs.A,y) + obs.b - mtimes(M,alpha), ub=0) -# mpc.set_nl_cons('obs_constr_2', sum1(alpha), ub=np.size(obs.A,0)-1) -# mpc.set_nl_cons('obs_constr_3', , ub=1, soft_constraint=True) -mpc.set_nl_cons('obs_constr_1', -sumsqr((x[:2]-SX([0,0]))**2), ub=-0.404) -# mpc.set_nl_cons('obs_constr_2', -sqrt((x[:2]-vertices[1])**2), ub=-0.01) -# mpc.set_nl_cons('obs_constr_3', -sqrt((x[:2]-vertices[2])**2), ub=-0.01) -# mpc.set_nl_cons('obs_constr_4', -sqrt((x[:2]-vertices[3])**2), ub=-0.01) -print(sumsqr((SX([0.2,0.3])-SX([0,0])**2))) - -tvp_template_mpc = mpc.get_tvp_template() - -# def tvp_fun_mpc(t_now): -# for k in range(Npred+1): -# temp = t_now + k -# if temp <= 15: -# tvp_template_mpc['_tvp', k, 'target'] = np.array([5, 5, 0, 0]) -# elif(15 < temp <=30): -# tvp_template_mpc['_tvp', k, 'target'] = np.array([-4, 0, 0, 0]) -# elif(temp > 30): -# tvp_template_mpc['_tvp', k, 'target'] = np.array([1, 0, 0, 0]) -# return tvp_template_mpc - -# positions = [(15,np.array([5, 5, 0, 0])), (30,np.array([-3, 0, 0, 0])), (50,np.array([4, 0, 0, 0])) ] -# current = 0 - -# def tvp_fun_mpc(t_now): -# global current -# next_position = positions[current] -# print("c= ", current) -# print("next_pos= ", next_position) -# if t_now == (next_position[0]-int(Npred/2)) and (current+1) < len(positions): -# current += 1 -# next_position = positions[current] - -# for k in range(Npred): -# temp = t_now + k -# if temp <= next_position[0]: -# tvp_template_mpc['_tvp', k, 'target'] = next_position[1] - -# return tvp_template_mpc - -# positions = [np.array([5, 5, 0, 0]) for x in range(0,15)] -# positions += [np.array([-3, 0, 0, 0]) for x in range(15,50)] -# positions += [np.array([4, 1, 0, 0]) for x in range(50,Nsim+Npred)] - - -def createPositionsVector(listOfPositions) -> list: - current_t = 0 - next_t = listOfPositions[0][0] - lentgh = len(listOfPositions) - positions = [] - for i in range(lentgh): - positions += [listOfPositions[i][1] for x in range(current_t,next_t)] - current_t = listOfPositions[i][0] - if i == lentgh-1: - next_t = Npred + Nsim - else: - next_t = listOfPositions[i+1][0] - positions += [listOfPositions[i][1] for x in range(current_t,next_t)] - return positions - -list = [(15,np.array([5, 5, 0, 0])), (30,np.array([-3, 0, 0, 0])), (50,np.array([4, 1, 0, 0])) ] -positions = createPositionsVector(list) -print(len(positions)) -print(positions) - -def tvp_fun_mpc(t_now): - global positions - for k in range(Npred): - # print(t_now+k) - tvp_template_mpc['_tvp', k, 'target'] = positions[int(t_now+k)] - - return tvp_template_mpc -mpc.set_tvp_fun(tvp_fun_mpc) - -mpc.setup() - -# Set up simulator -simulator = do_mpc.simulator.Simulator(model) -# Instead of supplying a dict with the splat operator (**), as with the optimizer.set_param(), -# we can also use keywords (and call the method multiple times, if necessary): -simulator.set_param(t_step = 0.1) -tvp_template_sim = simulator.get_tvp_template() -def tvp_fun_sim(t_now): - return tvp_template_sim -simulator.set_tvp_fun(tvp_fun_sim) -simulator.setup() -estimator = do_mpc.estimator.StateFeedback(model) - -# print(x0) -simulator.x0 = x0 -estimator.x0 = x0 -mpc.x0 = x0 -mpc.set_initial_guess() - -# Setting up the Graphic -# Customizing Matplotlib: -# mpl.rcParams['font.size'] = 18 -# mpl.rcParams['lines.linewidth'] = 3 -# mpl.rcParams['axes.grid'] = True -# # initializing the graphics module -# mpc_graphics = do_mpc.graphics.Graphics(mpc.data) -# sim_graphics = do_mpc.graphics.Graphics(simulator.data) - -# creating the figure -# We just want to create the plot and not show it right now. This "inline magic" supresses the output. -""" -Setup graphic: -""" -plt.ion() - -""" -Run MPC main loop: -""" -usim = DM(np.zeros((du, Nsim))); -ysim = DM(np.zeros((dy, Nsim+1))); -xsim = DM(np.zeros((dx, Nsim+1))); - -xsim[:, 0] = x0[:4] -for k in range(Nsim): - u0 = mpc.make_step(x0) - # u0[3:4] = to_binary(u0[3:4]) - y_next = simulator.make_step(u0) - x0 = estimator.make_step(y_next) - - usim[:,k] = u0 - ysim[:,k] = mtimes(C,x0[:4]) + mtimes(D,u0[:2]) - xsim[:,k+1] = mtimes(A,x0[:4]) + mtimes(B,u0[:2]) - - -# Plot parameters -fig, ax = plt.subplots(3, sharex=False, figsize=(16,9)) -fig.align_ylabels() - -ax[0].set_ylabel('x2') -ax[0].set_xlabel('x1') -ax[0].set_xlim(-float(np.amax(ysim[0,:].full())), float(np.amax(ysim[0,:].full())+1)) -ax[0].set_ylim(-float(np.amax(ysim[1,:].full())), float(np.amax(ysim[1,:].full())+1)) - -ax[1].set_xlabel('time [s]') -ax[1].set_ylabel('Vx(t)') -ax[1].set_xlim(0.0, float(Nsim)) -ax[1].set_ylim(float(np.amin(xsim[2:].full())-1), float(np.amax(xsim[2,:].full())+1)) - -ax[2].set_xlabel('time [s]') -ax[2].set_ylabel('Vy(t)') -ax[2].set_xlim(0.0, float(Nsim)) -ax[2].set_ylim(float(np.amin(xsim[3,:].full())-1), float(np.amax(xsim[3,:].full())+1)) - -polygon = Polygon(vertices) -ax[0].add_patch(polygon) -# ax[0].plot_polygon(vertices) -for k in range(Nsim): - ax[0].scatter(ysim[0,k].full(), ysim[1,k].full(), color='blue') - ax[1].stem(k, xsim[2,k].full(), '--', use_line_collection = True) - ax[2].stem(k, xsim[3,k].full(), '--', use_line_collection = True) - - fig.canvas.draw() - fig.canvas.flush_events() - -plt.savefig('anim.jpg') -plt.clf() - - -# Animating Results -def update(k): - ax[0].scatter(ysim[0,k].full(), ysim[1,k].full(), color='blue') - ax[1].stem(k, xsim[2,k].full(), '--', use_line_collection = True) - ax[2].stem(k, xsim[3,k].full(), '--', use_line_collection = True) - -anim = FuncAnimation(fig, update, frames=Nsim, repeat=False) -gif_writer = ImageMagickWriter(fps=3) -anim.save('anim.gif', writer=gif_writer) \ No newline at end of file diff --git a/target_tracking.py b/target_tracking.py deleted file mode 100644 index bd5bd4b..0000000 --- a/target_tracking.py +++ /dev/null @@ -1,92 +0,0 @@ - -from struct import Struct -from casadi import * -from numpy import zeros, array, eye, size, block, shape, dot, full -# from cplex import * - -# from matplotlib import pyplot as plt - -# System Declaration -# A = SX([ [1, 0, 0.5, 0], [0, 1, 0, 0.5], [0, 0, 1, 0], [0, 0, 0, 1] ]) -# print("A= ", A) -# B = SX([ [0, 0], [0, 0], [0.5, 0], [0, 0.5] ]) -# print("B= ", B) -# C = SX([ [1, 0, 0, 0], [0, 1, 0, 0] ]) -# print("C= ", C) -# D = SX.zeros(2,2) -# print("D= ", D) -A = array([ [1, 0, 0.5, 0], [0, 1, 0, 0.5], [0, 0, 1, 0], [0, 0, 0, 1] ]) -print("A= ", A) -B = array([ [0, 0], [0, 0], [0.5, 0], [0, 0.5] ]) -print("B= ", B) -C = array([ [1, 0, 0, 0], [0, 1, 0, 0] ]) -print("C= ", C) -D = zeros((2,2)) -print("D= ", D) - -print(size(B)) -dx, du = shape(B) -dy = size(C, 0) -print("dx= ", dx) -print("du= ", du) -print("dy= ", dy) - -# Constraints -umin = -10 -umax = 10 -delta_u_min = -0.1 -delta_u_max = 0.1 -ymin = -10; -ymax = 10; - -#Tuning matrixes -Q = eye(4) -print("Q=", Q) -# P = block([Q/10, 10*Q]) -# P = [Q*10, Q/10] -Qy = eye(dy) -P = Q*10 -print("P=", P) -R = 10 - -Npred = 5 -Nsim = 100 - -# Objective def -target = vertcat(0, 0, 0, 0) -x = MX.sym('x', dx) -u = MX.sym('u', du) - - -func = Q*sumsqr(x[:]-target) + R*sumsqr(u[:]) -f = Function('f', [x,u], [func], ['x','u'], ['objective']) -print("f(x,u)= ", f([1,1,1,1],[1,1])) - -# MPC Problem - -opti = Opti() - -u = opti.variable(du, Npred) -x = opti.variable(dx, Npred+1) -# y = opti.variable(dy, Npred+1) - -# print(x[:,1]*Q*x[:,1].T) - -# u_tmp = opti.variable(du, 1); -x_init =vertcat(15, 15, 0, 0); - - -opti.minimize(sumsqr(u) + sumsqr(x)) - -for k in range(Npred): - opti.subject_to(x[:,k+1] == f(x[:,k], u[:,k])) - -# opti.subject_to(umin <= u <= umax) -# opti.subject_to((x[:,Npred+1] == P*sumsqr(x[:,]-target))) -opti.subject_to(x[:,1] == x_init) - -# Solver -opti.solver('sqpmethod', {'qpsol': 'qrqp'}) -# opti.set_value(x_init, vertcat(15, 15, 0, 0)) -sol = opti.solve() -print("sol= ", sol) \ No newline at end of file diff --git a/target_tracking_dompc b/target_tracking_dompc deleted file mode 100644 index 34828aa..0000000 --- a/target_tracking_dompc +++ /dev/null @@ -1,232 +0,0 @@ -import numpy as np -# Import do_mpc package: -import do_mpc -from casadi import * -import matplotlib.pyplot as plt -import matplotlib as mpl -from matplotlib.animation import FuncAnimation, FFMpegWriter, ImageMagickWriter - -model_type = 'continuous' # either 'discrete' or 'continuous' -model = do_mpc.model.Model(model_type) - -# Model variables -phi_1 = model.set_variable(var_type='_x', var_name='phi_1', shape=(1,1)) -phi_2 = model.set_variable(var_type='_x', var_name='phi_2', shape=(1,1)) -phi_3 = model.set_variable(var_type='_x', var_name='phi_3', shape=(1,1)) -# Variables can also be vectors: -dphi = model.set_variable(var_type='_x', var_name='dphi', shape=(3,1)) -# Two states for the desired (set) motor position: -phi_m_1_set = model.set_variable(var_type='_u', var_name='phi_m_1_set') -phi_m_2_set = model.set_variable(var_type='_u', var_name='phi_m_2_set') -# Two additional states for the true motor position: -phi_1_m = model.set_variable(var_type='_x', var_name='phi_1_m', shape=(1,1)) -phi_2_m = model.set_variable(var_type='_x', var_name='phi_2_m', shape=(1,1)) - -# As shown in the table above, we can use Long names or short names for the variable type. -Theta_1 = model.set_variable('parameter', 'Theta_1') -Theta_2 = model.set_variable('parameter', 'Theta_2') -Theta_3 = model.set_variable('parameter', 'Theta_3') - -# target -trg = np.array([1, 1, 1, 0]) - -c = np.array([2.697, 2.66, 3.05, 2.86])*1e-3 -d = np.array([6.78, 8.01, 8.82])*1e-5 - -model.set_rhs('phi_1', dphi[0]) -model.set_rhs('phi_2', dphi[1]) -model.set_rhs('phi_3', dphi[2]) - -dphi_next = vertcat( - -c[0]/Theta_1*(phi_1-phi_1_m)-c[1]/Theta_1*(phi_1-phi_2)-d[0]/Theta_1*dphi[0], - -c[1]/Theta_2*(phi_2-phi_1)-c[2]/Theta_2*(phi_2-phi_3)-d[1]/Theta_2*dphi[1], - -c[2]/Theta_3*(phi_3-phi_2)-c[3]/Theta_3*(phi_3-phi_2_m)-d[2]/Theta_3*dphi[2], -) - -tau = 1e-2 -model.set_rhs('phi_1_m', 1/tau*(phi_m_1_set - phi_1_m)) -model.set_rhs('phi_2_m', 1/tau*(phi_m_2_set - phi_2_m)) - -model.set_rhs('dphi', dphi_next) -model.setup() - - -# set up mpc -mpc = do_mpc.controller.MPC(model) -# optimizer parameter -setup_mpc = { - 'n_horizon': 20, - 't_step': 0.1, - 'n_robust': 1, - 'store_full_solution': True, -} -mpc.set_param(**setup_mpc) - -mterm = 1/2*( (phi_1 - trg[0])**2 + (phi_2 - trg[1])**2 + (phi_3- trg[2])**2 ) -lterm = 1/2*( (phi_1 - trg[0])**2 + (phi_2 - trg[1])**2 + (phi_3- trg[2])**2 ) -print(mterm) -print(lterm) -mpc.set_objective(mterm=mterm, lterm=lterm) - -mpc.set_rterm( - phi_m_1_set=1e-2, - phi_m_2_set=1e-2 -) - -# Lower bounds on states: -mpc.bounds['lower','_x', 'phi_1'] = -2*np.pi -mpc.bounds['lower','_x', 'phi_2'] = -2*np.pi -mpc.bounds['lower','_x', 'phi_3'] = -2*np.pi -# Upper bounds on states -mpc.bounds['upper','_x', 'phi_1'] = 2*np.pi -mpc.bounds['upper','_x', 'phi_2'] = 2*np.pi -mpc.bounds['upper','_x', 'phi_3'] = 2*np.pi - -# Lower bounds on inputs: -mpc.bounds['lower','_u', 'phi_m_1_set'] = -2*np.pi -mpc.bounds['lower','_u', 'phi_m_2_set'] = -2*np.pi -# Lower bounds on inputs: -mpc.bounds['upper','_u', 'phi_m_1_set'] = 2*np.pi -mpc.bounds['upper','_u', 'phi_m_2_set'] = 2*np.pi - -# scaling variable (magnitudes) -mpc.scaling['_x', 'phi_1'] = 5 -mpc.scaling['_x', 'phi_2'] = 5 -mpc.scaling['_x', 'phi_3'] = 5 - -# Set uncertain values for robust based decision -inertia_mass_1 = 2.25*1e-4*np.array([1., 0.9, 1.1]) -inertia_mass_2 = 2.25*1e-4*np.array([1., 0.9, 1.1]) -inertia_mass_3 = 2.25*1e-4*np.array([1.]) - -mpc.set_uncertainty_values( - Theta_1 = inertia_mass_1, - Theta_2 = inertia_mass_2, - Theta_3 = inertia_mass_3 -) -mpc.setup() - - -# Set up simulator -simulator = do_mpc.simulator.Simulator(model) -# Instead of supplying a dict with the splat operator (**), as with the optimizer.set_param(), -# we can also use keywords (and call the method multiple times, if necessary): -simulator.set_param(t_step = 0.1) -p_template = simulator.get_p_template() -# function returning desired uncertain parameters -def p_fun(t_now): - p_template['Theta_1'] = 2.25e-4 - p_template['Theta_2'] = 2.25e-4 - p_template['Theta_3'] = 2.25e-4 - return p_template -# supply function to simulator -simulator.set_p_fun(p_fun) -simulator.setup() - -# Set up control loop -# initial state -x0 = np.pi*np.array([1, 1, -1.5, 1, -1, 1, 0, 0]).reshape(-1,1) -# print(x0) -simulator.x0 = x0 -mpc.x0 = x0 -mpc.set_initial_guess() - - -# Setting up the Graphic -# Customizing Matplotlib: -mpl.rcParams['font.size'] = 18 -mpl.rcParams['lines.linewidth'] = 3 -mpl.rcParams['axes.grid'] = True -# initializing the graphics module -mpc_graphics = do_mpc.graphics.Graphics(mpc.data) -sim_graphics = do_mpc.graphics.Graphics(simulator.data) -# creating the figure -# We just want to create the plot and not show it right now. This "inline magic" supresses the output. -fig, ax = plt.subplots(2, sharex=True, figsize=(16,9)) -fig.align_ylabels() - -for g in [sim_graphics, mpc_graphics]: - # Plot the angle positions (phi_1, phi_2, phi_2) on the first axis: - g.add_line(var_type='_x', var_name='phi_1', axis=ax[0]) - g.add_line(var_type='_x', var_name='phi_2', axis=ax[0]) - g.add_line(var_type='_x', var_name='phi_3', axis=ax[0]) - - # Plot the set motor positions (phi_m_1_set, phi_m_2_set) on the second axis: - g.add_line(var_type='_u', var_name='phi_m_1_set', axis=ax[1]) - g.add_line(var_type='_u', var_name='phi_m_2_set', axis=ax[1]) - - -ax[0].set_ylabel('angle position [rad]') -ax[1].set_ylabel('motor angle [rad]') -ax[1].set_xlabel('time [s]') - - -# Running simulator -# We start investigating the do-mpc simulator and the graphics package -# by simulating the autonomous system without control inputs (u=0). This can be done as follows: -u0 = np.zeros((2,1)) -for i in range(200): - simulator.make_step(u0) -sim_graphics.plot_results() -# Reset the limits on all axes in graphic to show the data. -sim_graphics.reset_axes() -# Show the figure: -fig.show() - -# Running Optimizer -u0 = mpc.make_step(x0) -sim_graphics.clear() - -mpc_graphics.plot_predictions() -mpc_graphics.reset_axes() -# Show the figure: -fig.show() - - -# Appearance of figures -# change lines colors for the three states: -for line_i in mpc_graphics.pred_lines['_x', 'phi_1']: line_i.set_color('#1f77b4') # blue -for line_i in mpc_graphics.pred_lines['_x', 'phi_2']: line_i.set_color('#ff7f0e') # orange -for line_i in mpc_graphics.pred_lines['_x', 'phi_3']: line_i.set_color('#2ca02c') # green -# Change the color for the two inputs: -for line_i in mpc_graphics.pred_lines['_u', 'phi_m_1_set']: line_i.set_color('#1f77b4') -for line_i in mpc_graphics.pred_lines['_u', 'phi_m_2_set']: line_i.set_color('#ff7f0e') -# Make all predictions transparent: -for line_i in mpc_graphics.pred_lines.full: line_i.set_alpha(0.2) -# legend -# Get line objects (note sum of lists creates a concatenated list) -lines = sim_graphics.result_lines['_x', 'phi_1']+sim_graphics.result_lines['_x', 'phi_2']+sim_graphics.result_lines['_x', 'phi_3'] -ax[0].legend(lines,'123',title='disc') -# also set legend for second subplot: -lines = sim_graphics.result_lines['_u', 'phi_m_1_set']+sim_graphics.result_lines['_u', 'phi_m_2_set'] -ax[1].legend(lines,'12',title='motor') - - -# Running Control loop -simulator.reset_history() -simulator.x0 = x0 -mpc.reset_history() -# capture -for i in range(20): - u0 = mpc.make_step(x0) - x0 = simulator.make_step(u0) -# Plot predictions from t=0 -mpc_graphics.plot_predictions(t_ind=0) -# Plot results until current time -sim_graphics.plot_results() -sim_graphics.reset_axes() -# fig.show - - -# Animating Results -def update(t_ind): - sim_graphics.plot_results(t_ind) - mpc_graphics.plot_predictions(t_ind) - mpc_graphics.reset_axes() - -anim = FuncAnimation(fig, update, frames=20, repeat=False) -gif_writer = ImageMagickWriter(fps=3) -anim.save('anim.gif', writer=gif_writer) -plt.show() - -SX.binary() \ No newline at end of file diff --git a/utils/__init__.py b/utils/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/utils/fileManagement.py b/utils/fileManagement.py deleted file mode 100644 index 2b48a44..0000000 --- a/utils/fileManagement.py +++ /dev/null @@ -1,20 +0,0 @@ -import tkinter as tk -from tkinter import filedialog - -def getFile(): - """ getFile() --> filePath - Prompt user for json file - """ - try: - filetypes = ( - ('json files', '*.json'), - ) - root = tk.Tk() - root.withdraw() - return(filedialog.askopenfilename(filetypes=filetypes)) - except: - print("fileManagement.getFile(): Error getting filePath") - return None - - -