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