From 55a42b2be769bbfbd5b697338c6729f06a3d17cc Mon Sep 17 00:00:00 2001 From: YanzhenXiangRobotics Date: Tue, 25 Apr 2023 20:28:44 +0200 Subject: ADD: pass system modeling test --- install_mpt3.m | 108 +++++++++++++++++++++++++++++++++++++ templates/generate_constraints.m | 8 +++ templates/generate_params.m | 28 ++++++++++ templates/generate_system.m | 8 +++ templates/generate_system_cont.m | 11 ++++ templates/generate_system_scaled.m | 4 ++ 6 files changed, 167 insertions(+) create mode 100644 install_mpt3.m diff --git a/install_mpt3.m b/install_mpt3.m new file mode 100644 index 0000000..97981dd --- /dev/null +++ b/install_mpt3.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/generate_constraints.m b/templates/generate_constraints.m index 892b706..f87ac26 100644 --- a/templates/generate_constraints.m +++ b/templates/generate_constraints.m @@ -8,4 +8,12 @@ function [H_u, h_u, H_x, h_x] = generate_constraints(params) % YOUR CODE HERE + u_max = params.constraints.MaxAbsThrust; + h_u = u_max * ones(6,1); + H_u = [eye(3);-eye(3)]; + + s_max = params.constraints.MaxAbsPositionXZ; + y_max = params.constraints.MaxAbsPositionY; + h_x = [s_max;y_max;s_max;s_max;y_max;s_max]; + H_x = [eye(3);-eye(3)]; end \ No newline at end of file diff --git a/templates/generate_params.m b/templates/generate_params.m index ef51366..e2e6024 100644 --- a/templates/generate_params.m +++ b/templates/generate_params.m @@ -41,5 +41,33 @@ params.exercise = struct( ... ); % YOUR CODE HERE +[Ac, Bc] = generate_system_cont(params); +[A_tilta, B_tilta] = generate_system(Ac, Bc, params); +[A, B] = generate_system_scaled(A_tilta, B_tilta, params); +[Hu, hu, Hx, hx] = generate_constraints(params); + +params.model.A = A; +params.model.B = B; + +params.constraints.InputMatrix = Hu; +params.constraints.InputRHS = hu; +params.constraints.StateMatrix = Hx; +params.constraints.StateRHS = hx; + +% new_model_struct = struct(... +% 'A', A, ... +% 'B', B ... +% ); + +% new_constr_struct = struct(... +% 'InputMatrix', Hu,... +% 'InputRHS', hu,... +% 'StateMatrix', Hx,... +% 'StateRHS', hx ... +% ) + + +% params.model = [params.model new_model_struct]; +% params.constraints = [params.constraints, new_constr_struct]; end diff --git a/templates/generate_system.m b/templates/generate_system.m index 9deb347..c4dadc2 100644 --- a/templates/generate_system.m +++ b/templates/generate_system.m @@ -8,4 +8,12 @@ function [A, B] = generate_system(Ac, Bc, params) % YOUR CODE HERE + % Ts = 600; + sysc = ss(Ac, Bc, [], []); + Ts = params.model.TimeStep; + sysd = c2d(sysc,Ts); + % A = eye(params.model.nx) + Ac * Ts; + % B = Bc * Ts; + A = sysd.A; + B = sysd.B; end \ No newline at end of file diff --git a/templates/generate_system_cont.m b/templates/generate_system_cont.m index 2d3ee79..770fd8e 100644 --- a/templates/generate_system_cont.m +++ b/templates/generate_system_cont.m @@ -8,4 +8,15 @@ function [Ac, Bc] = generate_system_cont(params) % YOUR CODE HERE + miu = params.model.GravitationalParameter; + r = params.model.TargetRadius; + omega_n = sqrt(miu/(r^3)); + m = params.model.Mass; + Ac = [0,0,0,1,0,0; + 0,0,0,0,1,0; + 0,0,0,0,0,1; + 3*omega_n^2,0,0,0,2*omega_n,0; + 0,0,0,-2*omega_n,0,0; + 0,0,-omega_n^2,0,0,0]; + Bc = [zeros(3,3);1/m*eye(3)]; end \ No newline at end of file diff --git a/templates/generate_system_scaled.m b/templates/generate_system_scaled.m index eac8db8..38714d2 100644 --- a/templates/generate_system_scaled.m +++ b/templates/generate_system_scaled.m @@ -8,4 +8,8 @@ function [A,B] = generate_system_scaled(At,Bt,params) % YOUR CODE HERE + % V = diag([1e-6, 1e-6, 1e-6, 1e-3, 1e-3, 1e-3]); + V = params.model.ScalingMatrix; + A = V*At*inv(V); + B = V*Bt; end \ No newline at end of file -- cgit v1.2.1