From 657966bf73fdb50fd1c7b55b15766e3eadfb7d4a Mon Sep 17 00:00:00 2001 From: YanzhenXiangRobotics <54230111+YanzhenXiangRobotics@users.noreply.github.com> Date: Mon, 15 May 2023 13:13:37 +0200 Subject: ADD: raw 30 --- Installation.m | 108 +++++++++++++++++++++++++++++++++++++++++++++++++++ templates/MPC_TUBE.m | 41 ++++++++++++++++++- 2 files changed, 148 insertions(+), 1 deletion(-) create mode 100644 Installation.m diff --git a/Installation.m b/Installation.m new file mode 100644 index 0000000..97981dd --- /dev/null +++ b/Installation.m @@ -0,0 +1,108 @@ +%% installation of tbxmanager with all submodels required for MPT +% + +clc; +disp('----------------------------------------------'); +disp('Installation of MPT using the Toolbox manager.'); +disp('----------------------------------------------'); +disp(' '); +fprintf(['Choose the installation directory where to install the Toolbox manager.\n',... + 'A new folder "tbxmanager" is going to be created in the specified location.\n',... + 'If you do not specify the folder, the Toolbox manager will be installed in the current directory.\n']); + +% get the installation folder +default_dir = pwd; +c = uigetdir(pwd); +if isequal(c,0); + fprintf(['No directory has been provided.\n',... + 'Installing the toolbox manager in the current directory "%s"?\n'],default_dir); + c = default_dir; +end + +% create a new directory in that folder +d = [c,filesep,'tbxmanager']; +if isequal(exist(d,'dir'),7) + error('The installation directory "%s" already exists.\nPlease, remove or rename the folder or change the installation path.',d); +end +disp('Creating the directory "tbxmanager".'); +out = mkdir(d); +if ~out + error(['An error appear when trying to create the folder "%s".\n',... + 'Please, install the Toolbox manager manually.'],c); +end + +% enter that directory +cd(d); + +% remove MPT2 or YALMIP +disp(' '); +disp('Removing toolboxes that may conflict with MPT from the Matlab path.'); +rmpath(genpath(fileparts(which('mpt_init')))); +rmpath(genpath(fileparts(which('yalmipdemo')))); + + +% download the tbxmanager +disp(' '); +disp('Downloading the Toolbox manager from the internet.'); +[f, c] = urlwrite('http://www.tbxmanager.com/tbxmanager.m', 'tbxmanager.m'); +rehash; + +if isequal(c,0) + error('Could not download the Toolbox manager from the internet. The installation cannot continue.'); +end + +% install all required modules +tbxmanager install mpt mptdoc cddmex fourier glpkmex hysdel lcp sedumi yalmip + +% create the initialization file to set the path +disp(' '); +disp('Creating the initialization file "startup.m".'); +p = which('startup.m'); +if isempty(p) + p = [d,filesep,'startup.m']; +end +fid = fopen(p,'a'); +if isequal(fid,-1) + error(['Could not modify the initialization file "startup.m".',... + 'Edit this file in the folder "%s" manually and insert there the line: tbxmanager restorepath.'],p); +end +fprintf(fid,'tbxmanager restorepath\n'); +fclose(fid); +disp('File has been created.'); + +% get back to the original directory +cd(default_dir); + +% add path to tbxmanager +disp(' '); +disp('Adding path to Matlab.'); +addpath(d); + +% save path for future +disp(' '); +disp('Saving path for future sessions.'); +status = savepath; + +if status + fprintf('Could not save the path to a default location,\nplease provide a location where you want to save the path.'); + cn = uigetdir(pwd); + if isequal(cn,0) + disp(' '); + fprintf('No directory specified, saving the path to the current directory "%s".\n\n',default_dir); + cn = default_dir; + end + sn = savepath([cn,filesep,'pathdef.m']); + if sn + error(['Could not save the path automatically.\n',... + 'Please, open the "Set Path" button in the Matlab menu and save the path manually to some location.']); + end +end + +disp(' '); +disp('Installation finished.'); +disp('Next time you start Matlab the toolboxes will be automatically initialized.'); + +% initialize MPT +disp(' '); +disp('Initializing the MPT.') +mpt_init; diff --git a/templates/MPC_TUBE.m b/templates/MPC_TUBE.m index bd62044..83b056b 100644 --- a/templates/MPC_TUBE.m +++ b/templates/MPC_TUBE.m @@ -17,6 +17,44 @@ classdef MPC_TUBE obj.K_tube = K_tube; % YOUR CODE HERE + nx = params.model.nx; + nu = params.model.nu; + + A = params.model.A; + B = params.model.B; + + [~,P,~] = dlqr(A,B,Q,R); + + V = sdpvar(repmat(nu,1,N),ones(1,N),'full'); + Z = sdpvar(repmat(nx,1,N+1),ones(1,N+1),'full'); + + H_x = params.constraints.StateMatrix; + h_x = params.constraints.StateRHS; + H_u = params.constraints.InputMatrix; + h_u = params.constraints.InputRHS; + + P_x_tightened = Polyhedron('A',H_x,'b',h_x).minus(Polyhedron('A',H_tube,'b',h_tube)); + H_z = P_x_tightened.A; + h_z = P_x_tightened.b; + + P_u_tightened = Polyhedron('A',H_u,'b',h_u).minus(K_tube*Polyhedron('A',H_tube,'b',h_tube)); + H_v = P_u_tightened.A; + h_v = P_u_tightened.b; + + X0 = sdpvar(nx,1,'full'); + constraints = [H_tube*(Z{1}-X0) <= h_tube]; + objective = 0; + for k = 1:N + constraints = [ ... + constraints, ... + Z{k+1} == A*Z{k} + B*V{k} , ... + H_z * Z{k} <= h_z, ... + H_v * V{k} <= h_v ... + ]; + objective = objective + Z{k}'*Q*Z{k} + V{k}'*R*V{k}; + end + constraints = [constraints, H_N * Z{end} <= h_N]; + objective = objective + Z{end}'*P*Z{end}; opts = sdpsettings('verbose',1,'solver','quadprog'); obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{V{1} Z{1} objective}); @@ -28,7 +66,8 @@ classdef MPC_TUBE [optimizer_out,errorcode] = obj.yalmip_optimizer(x); solvetime = toc; % YOUR CODE HERE - + [u, objective] = optimizer_out{:}; + feasible = true; if (errorcode ~= 0) feasible = false; -- cgit v1.2.1