From bbf8c17a58001bfe15948e3b2a31ca2a1a95f928 Mon Sep 17 00:00:00 2001 From: YanzhenXiangRobotics Date: Tue, 25 Apr 2023 16:42:53 +0200 Subject: ADD: init proj --- project2023.pdf | Bin 0 -> 360222 bytes templates/.MATLABDriveTag | 1 + templates/LICENSE.txt | 7 ++ templates/LQR.m | 27 ++++++ templates/MPC.m | 45 +++++++++ templates/MPC_TE.m | 36 +++++++ templates/MPC_TE_forces.m | 38 ++++++++ templates/MPC_TS.m | 36 +++++++ templates/MPC_TS_SC.m | 36 +++++++ templates/MPC_TUBE.m | 40 ++++++++ templates/compute_minRPI.m | 11 +++ templates/compute_tightening.m | 11 +++ templates/compute_tube_controller.m | 11 +++ templates/generate_constraints.m | 11 +++ templates/generate_disturbances.m | 11 +++ templates/generate_params.m | 45 +++++++++ templates/generate_params_z.m | 55 +++++++++++ templates/generate_system.m | 11 +++ templates/generate_system_cont.m | 11 +++ templates/generate_system_scaled.m | 11 +++ templates/lqr_maxPI.m | 12 +++ templates/lqr_tuning.m | 11 +++ templates/plot_trajectory.m | 126 +++++++++++++++++++++++++ templates/plot_trajectory_z.m | 78 +++++++++++++++ templates/simulate.m | 14 +++ templates/simulate_uncertain.m | 11 +++ templates/traj_constraints.m | 12 +++ templates/traj_cost.m | 11 +++ testing/.MATLABDriveTag | 1 + testing/TestFunction.p | Bin 0 -> 3855 bytes testing/TestFunction_LQR.p | Bin 0 -> 970 bytes testing/TestFunction_MPC.p | Bin 0 -> 1717 bytes testing/TestFunction_MPC_TE.p | Bin 0 -> 1659 bytes testing/TestFunction_MPC_TE_forces.p | Bin 0 -> 1670 bytes testing/TestFunction_MPC_TS.p | Bin 0 -> 1761 bytes testing/TestFunction_MPC_TS_SC.p | Bin 0 -> 2187 bytes testing/TestFunction_MPC_TS_SC_script.p | Bin 0 -> 1179 bytes testing/TestFunction_MPC_TUBE.p | Bin 0 -> 1857 bytes testing/TestFunction_MPC_TUBE_script.p | Bin 0 -> 1033 bytes testing/TestFunction_compute_minRPI.p | Bin 0 -> 593 bytes testing/TestFunction_compute_tightening.p | Bin 0 -> 1000 bytes testing/TestFunction_compute_tube_controller.p | Bin 0 -> 647 bytes testing/TestFunction_generate_constraints.p | Bin 0 -> 763 bytes testing/TestFunction_generate_disturbances.p | Bin 0 -> 864 bytes testing/TestFunction_generate_params.p | Bin 0 -> 1247 bytes testing/TestFunction_generate_system.p | Bin 0 -> 840 bytes testing/TestFunction_generate_system_cont.p | Bin 0 -> 852 bytes testing/TestFunction_generate_system_scaled.p | Bin 0 -> 886 bytes testing/TestFunction_lqr_maxPI.p | Bin 0 -> 785 bytes testing/TestFunction_lqr_tuning.p | Bin 0 -> 1529 bytes testing/TestFunction_lqr_tuning_script.p | Bin 0 -> 906 bytes testing/TestFunction_simulate.p | Bin 0 -> 983 bytes testing/TestFunction_simulate_uncertain.p | Bin 0 -> 1251 bytes testing/TestFunction_traj_constraints.p | Bin 0 -> 1405 bytes testing/TestFunction_traj_cost.p | Bin 0 -> 694 bytes testing/linearController.p | Bin 0 -> 183 bytes testing/run_tests.p | Bin 0 -> 3072 bytes 57 files changed, 730 insertions(+) create mode 100644 project2023.pdf create mode 100644 templates/.MATLABDriveTag create mode 100644 templates/LICENSE.txt create mode 100644 templates/LQR.m create mode 100644 templates/MPC.m create mode 100644 templates/MPC_TE.m create mode 100644 templates/MPC_TE_forces.m create mode 100644 templates/MPC_TS.m create mode 100644 templates/MPC_TS_SC.m create mode 100644 templates/MPC_TUBE.m create mode 100644 templates/compute_minRPI.m create mode 100644 templates/compute_tightening.m create mode 100644 templates/compute_tube_controller.m create mode 100644 templates/generate_constraints.m create mode 100644 templates/generate_disturbances.m create mode 100644 templates/generate_params.m create mode 100644 templates/generate_params_z.m create mode 100644 templates/generate_system.m create mode 100644 templates/generate_system_cont.m create mode 100644 templates/generate_system_scaled.m create mode 100644 templates/lqr_maxPI.m create mode 100644 templates/lqr_tuning.m create mode 100644 templates/plot_trajectory.m create mode 100644 templates/plot_trajectory_z.m create mode 100644 templates/simulate.m create mode 100644 templates/simulate_uncertain.m create mode 100644 templates/traj_constraints.m create mode 100644 templates/traj_cost.m create mode 100644 testing/.MATLABDriveTag create mode 100644 testing/TestFunction.p create mode 100644 testing/TestFunction_LQR.p create mode 100644 testing/TestFunction_MPC.p create mode 100644 testing/TestFunction_MPC_TE.p create mode 100644 testing/TestFunction_MPC_TE_forces.p create mode 100644 testing/TestFunction_MPC_TS.p create mode 100644 testing/TestFunction_MPC_TS_SC.p create mode 100644 testing/TestFunction_MPC_TS_SC_script.p create mode 100644 testing/TestFunction_MPC_TUBE.p create mode 100644 testing/TestFunction_MPC_TUBE_script.p create mode 100644 testing/TestFunction_compute_minRPI.p create mode 100644 testing/TestFunction_compute_tightening.p create mode 100644 testing/TestFunction_compute_tube_controller.p create mode 100644 testing/TestFunction_generate_constraints.p create mode 100644 testing/TestFunction_generate_disturbances.p create mode 100644 testing/TestFunction_generate_params.p create mode 100644 testing/TestFunction_generate_system.p create mode 100644 testing/TestFunction_generate_system_cont.p create mode 100644 testing/TestFunction_generate_system_scaled.p create mode 100644 testing/TestFunction_lqr_maxPI.p create mode 100644 testing/TestFunction_lqr_tuning.p create mode 100644 testing/TestFunction_lqr_tuning_script.p create mode 100644 testing/TestFunction_simulate.p create mode 100644 testing/TestFunction_simulate_uncertain.p create mode 100644 testing/TestFunction_traj_constraints.p create mode 100644 testing/TestFunction_traj_cost.p create mode 100644 testing/linearController.p create mode 100644 testing/run_tests.p diff --git a/project2023.pdf b/project2023.pdf new file mode 100644 index 0000000..bde0b0a Binary files /dev/null and b/project2023.pdf differ diff --git a/templates/.MATLABDriveTag b/templates/.MATLABDriveTag new file mode 100644 index 0000000..7aad4f3 --- /dev/null +++ b/templates/.MATLABDriveTag @@ -0,0 +1 @@ +f7c7757b-ada0-4bff-af70-1240edb65e6d \ No newline at end of file diff --git a/templates/LICENSE.txt b/templates/LICENSE.txt new file mode 100644 index 0000000..685f12c --- /dev/null +++ b/templates/LICENSE.txt @@ -0,0 +1,7 @@ +Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. + +All rights reserved. + +The files are provided for educational purposes only and should be distributed only to the attendees of the class "151-0660-00L Model Predictive Control FS2023". + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/templates/LQR.m b/templates/LQR.m new file mode 100644 index 0000000..2a79da9 --- /dev/null +++ b/templates/LQR.m @@ -0,0 +1,27 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +classdef LQR + properties + K + end + + methods + %constructor + function obj = LQR(Q,R,params) + % YOUR CODE HERE + % obj.K = ... (save feedback matrix for use in eval function) + end + + function [u, ctrl_info] = eval(obj,x) + % YOUR CODE HERE + % u = ... + ctrl_info = struct('ctrl_feas',true); + end + end +end \ No newline at end of file diff --git a/templates/MPC.m b/templates/MPC.m new file mode 100644 index 0000000..3e9d2f1 --- /dev/null +++ b/templates/MPC.m @@ -0,0 +1,45 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +classdef MPC + properties + yalmip_optimizer + end + + methods + function obj = MPC(Q,R,N,params) + nu = params.model.nu; + nx = params.model.nx; + + % define optimization variables + U = sdpvar(repmat(nu,1,N),ones(1,N),'full'); + X0 = sdpvar(nx,1,'full'); + + % YOUR CODE HERE + + opts = sdpsettings('verbose',1,'solver','quadprog'); + obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective}); + end + + function [u, ctrl_info] = eval(obj,x) + %% evaluate control action by solving MPC problem, e.g. + tic; + [optimizer_out,errorcode] = obj.yalmip_optimizer(x); + solvetime = toc; + + [u, objective] = optimizer_out{:}; + + feasible = true; + if (errorcode ~= 0) + feasible = false; + end + + ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime); + end + end +end \ No newline at end of file diff --git a/templates/MPC_TE.m b/templates/MPC_TE.m new file mode 100644 index 0000000..e0b55d2 --- /dev/null +++ b/templates/MPC_TE.m @@ -0,0 +1,36 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +classdef MPC_TE + properties + yalmip_optimizer + end + + methods + function obj = MPC_TE(Q,R,N,params) + % YOUR CODE HERE + opts = sdpsettings('verbose',1,'solver','quadprog'); + obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective}); + end + + function [u, ctrl_info] = eval(obj,x) + %% evaluate control action by solving MPC problem, e.g. + tic; + [optimizer_out,errorcode] = obj.yalmip_optimizer(x); + solvetime = toc; + [u, objective] = optimizer_out{:}; + + feasible = true; + if (errorcode ~= 0) + feasible = false; + end + + ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime); + end + end +end \ No newline at end of file diff --git a/templates/MPC_TE_forces.m b/templates/MPC_TE_forces.m new file mode 100644 index 0000000..5625f32 --- /dev/null +++ b/templates/MPC_TE_forces.m @@ -0,0 +1,38 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +classdef MPC_TE_forces + properties + forces_optimizer + end + + methods + function obj = MPC_TE_forces(Q,R,N,params) + % YOUR CODE HERE + opts = getOptions('forcesSolver'); + opts.printlevel = 0; + obj.forces_optimizer = % YOUR CODE HERE + end + + function [u, ctrl_info] = eval(obj,x) + %% evaluate control action by solving MPC problem, e.g. + [optimizer_out,errorcode,info] = obj.forces_optimizer(x); + u = optimizer_out; + objective = info.pobj; + solvetime = info.solvetime; + + feasible = true; + if any(errorcode ~= 1) + feasible = false; + warning('MPC infeasible'); + end + + ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime); + end + end +end \ No newline at end of file diff --git a/templates/MPC_TS.m b/templates/MPC_TS.m new file mode 100644 index 0000000..05f92ff --- /dev/null +++ b/templates/MPC_TS.m @@ -0,0 +1,36 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +classdef MPC_TS + properties + yalmip_optimizer + end + + methods + function obj = MPC_TS(Q,R,N,H,h,params) + % YOUR CODE HERE + opts = sdpsettings('verbose',1,'solver','quadprog'); + obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective}); + end + + function [u, ctrl_info] = eval(obj,x) + %% evaluate control action by solving MPC problem, e.g. + tic; + [optimizer_out,errorcode] = obj.yalmip_optimizer(x); + solvetime = toc; + [u, objective] = optimizer_out{:}; + + feasible = true; + if (errorcode ~= 0) + feasible = false; + end + + ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime); + end + end +end \ No newline at end of file diff --git a/templates/MPC_TS_SC.m b/templates/MPC_TS_SC.m new file mode 100644 index 0000000..0e64767 --- /dev/null +++ b/templates/MPC_TS_SC.m @@ -0,0 +1,36 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +classdef MPC_TS_SC + properties + yalmip_optimizer + end + + methods + function obj = MPC_TS_SC(Q,R,N,H,h,S,v,params) + % YOUR CODE HERE + opts = sdpsettings('verbose',1,'solver','quadprog','quadprog.TolFun',1e-8); + obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{U{1} objective}); + end + + function [u, ctrl_info] = eval(obj,x) + %% evaluate control action by solving MPC problem, e.g. + tic; + [optimizer_out,errorcode] = obj.yalmip_optimizer(x); + solvetime = toc; + [u, objective] = optimizer_out{:}; + + feasible = true; + if (errorcode ~= 0) + feasible = false; + end + + ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime); + end + end +end \ No newline at end of file diff --git a/templates/MPC_TUBE.m b/templates/MPC_TUBE.m new file mode 100644 index 0000000..bd62044 --- /dev/null +++ b/templates/MPC_TUBE.m @@ -0,0 +1,40 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +classdef MPC_TUBE + properties + yalmip_optimizer + K_tube + end + + methods + function obj = MPC_TUBE(Q,R,N,H_N,h_N,H_tube,h_tube,K_tube,params) + obj.K_tube = K_tube; + + % YOUR CODE HERE + + opts = sdpsettings('verbose',1,'solver','quadprog'); + obj.yalmip_optimizer = optimizer(constraints,objective,opts,X0,{V{1} Z{1} objective}); + end + + function [u, ctrl_info] = eval(obj,x) + %% evaluate control action by solving MPC problem, e.g. + tic; + [optimizer_out,errorcode] = obj.yalmip_optimizer(x); + solvetime = toc; + % YOUR CODE HERE + + feasible = true; + if (errorcode ~= 0) + feasible = false; + end + + ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime); + end + end +end \ No newline at end of file diff --git a/templates/compute_minRPI.m b/templates/compute_minRPI.m new file mode 100644 index 0000000..0b4ffdb --- /dev/null +++ b/templates/compute_minRPI.m @@ -0,0 +1,11 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [H_tube,h_tube,n_iter] = compute_minRPI(K_tube,params) + % YOUR CODE HERE +end \ No newline at end of file diff --git a/templates/compute_tightening.m b/templates/compute_tightening.m new file mode 100644 index 0000000..8919d12 --- /dev/null +++ b/templates/compute_tightening.m @@ -0,0 +1,11 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function params = compute_tightening(K_tube,H_tube,h_tube,params) + % YOUR CODE HERE +end \ No newline at end of file diff --git a/templates/compute_tube_controller.m b/templates/compute_tube_controller.m new file mode 100644 index 0000000..adca95d --- /dev/null +++ b/templates/compute_tube_controller.m @@ -0,0 +1,11 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function K_tube = compute_tube_controller(p,params) + % YOUR CODE HERE +end \ No newline at end of file diff --git a/templates/generate_constraints.m b/templates/generate_constraints.m new file mode 100644 index 0000000..892b706 --- /dev/null +++ b/templates/generate_constraints.m @@ -0,0 +1,11 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [H_u, h_u, H_x, h_x] = generate_constraints(params) + % YOUR CODE HERE +end \ No newline at end of file diff --git a/templates/generate_disturbances.m b/templates/generate_disturbances.m new file mode 100644 index 0000000..570a4c9 --- /dev/null +++ b/templates/generate_disturbances.m @@ -0,0 +1,11 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function Wt = generate_disturbances(params) + % YOUR CODE HERE +end \ No newline at end of file diff --git a/templates/generate_params.m b/templates/generate_params.m new file mode 100644 index 0000000..ef51366 --- /dev/null +++ b/templates/generate_params.m @@ -0,0 +1,45 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [params] = generate_params() +params = struct(); + +Tf = 60*60*24 * 2; % = 2 days +dt = 60 * 10; % = 10 minutes + +% model +params.model = struct(... + 'nx', 6, ... + 'nu', 3, ... + 'Mass', 300, ... + 'GravitationalParameter', 3.986e14, ... + 'ScalingMatrix', [1e-6*eye(3), zeros(3); zeros(3), 1e-3*eye(3)], ... + 'TargetRadius', 7000e3, ... + 'TimeStep', dt, ... + 'HorizonLength', ceil(Tf / dt), ... + 'InitialConditionA', [-15e-3; -400e-3; 24.4e-3; 0; 0.0081; 0], ... + 'InitialConditionB', [-20e-3; 400e-3; 24.4e-3; 0; 0.0108; 0], ... + 'InitialConditionC', [0.02; 0.01; -0.005; 0; 0; 0] ... +); + +% constraints +params.constraints = struct(... + 'MaxAbsPositionXZ', 0.1, ... + 'MaxAbsPositionY', 1, ... + 'MaxAbsThrust', 1, ... + 'MaxFinalPosDiff' , 3e-4, ... + 'MaxFinalVelDiff', 1e-3 ... +); + +params.exercise = struct( ... + 'QdiagOptA', [94.0; 0.1579; 300; 0.01; 0.10; 0.10] ... +); + +% YOUR CODE HERE + +end diff --git a/templates/generate_params_z.m b/templates/generate_params_z.m new file mode 100644 index 0000000..a7cfaa9 --- /dev/null +++ b/templates/generate_params_z.m @@ -0,0 +1,55 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [params_z] = generate_params_z(params) +% initialize params_z +params_z = params; + +% add initial condition of z-subsystem +params_z.model = rmfield(params_z.model,{'InitialConditionA', ... + 'InitialConditionB', ... + 'InitialConditionC'}); +params_z.model.InitialConditionA_z = [-0.07;0.04]; % TODO: tune + +% define projection matrices +T_z_x = [0 0 1 0 0 0; + 0 0 0 0 0 1]; +T_z_u = [0 0 1]; + +% projected system dynamics +params_z.model.A = T_z_x*params.model.A*T_z_x'; +params_z.model.B = T_z_x*params.model.B*T_z_u'; +params_z.model.nx = 2; +params_z.model.nu = 1; + +% state constraints +Hx = params.constraints.StateMatrix * T_z_x'; +hx = params.constraints.StateRHS; +X = Polyhedron('A',Hx,'b',hx); +X.minHRep(); + +params_z.constraints.StateMatrix = X.A; +params_z.constraints.StateRHS = X.b; + +% input constraints +Hu = params.constraints.InputMatrix * T_z_u'; +hu = params.constraints.InputRHS; +U = Polyhedron('A',Hu,'b',hu); + +params_z.constraints.InputMatrix = U.A; +params_z.constraints.InputRHS = U.b; + +% disturbance constraints +max_disturbance = 1e-4; % TODO: chose +params_z.constraints.MaxAbsDisturbance = max_disturbance; +H_w = kron(eye(params_z.model.nx),[1;-1]); +h_w = ones(2*params_z.model.nx, 1) * max_disturbance; + +params_z.constraints.DisturbanceMatrix = H_w; +params_z.constraints.DisturbanceRHS = h_w; +end diff --git a/templates/generate_system.m b/templates/generate_system.m new file mode 100644 index 0000000..9deb347 --- /dev/null +++ b/templates/generate_system.m @@ -0,0 +1,11 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [A, B] = generate_system(Ac, Bc, params) + % YOUR CODE HERE +end \ No newline at end of file diff --git a/templates/generate_system_cont.m b/templates/generate_system_cont.m new file mode 100644 index 0000000..2d3ee79 --- /dev/null +++ b/templates/generate_system_cont.m @@ -0,0 +1,11 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [Ac, Bc] = generate_system_cont(params) + % YOUR CODE HERE +end \ No newline at end of file diff --git a/templates/generate_system_scaled.m b/templates/generate_system_scaled.m new file mode 100644 index 0000000..eac8db8 --- /dev/null +++ b/templates/generate_system_scaled.m @@ -0,0 +1,11 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [A,B] = generate_system_scaled(At,Bt,params) + % YOUR CODE HERE +end \ No newline at end of file diff --git a/templates/lqr_maxPI.m b/templates/lqr_maxPI.m new file mode 100644 index 0000000..efa335a --- /dev/null +++ b/templates/lqr_maxPI.m @@ -0,0 +1,12 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [H, h] = lqr_maxPI(Q,R,params) + % YOUR CODE HERE +end + diff --git a/templates/lqr_tuning.m b/templates/lqr_tuning.m new file mode 100644 index 0000000..4a26158 --- /dev/null +++ b/templates/lqr_tuning.m @@ -0,0 +1,11 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [tuning_struct, i_opt] = lqr_tuning(x0,Q,params) + % YOUR CODE HERE +end \ No newline at end of file diff --git a/templates/plot_trajectory.m b/templates/plot_trajectory.m new file mode 100644 index 0000000..9bcb696 --- /dev/null +++ b/templates/plot_trajectory.m @@ -0,0 +1,126 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [fig_time,axes_time,fig_pos,axes_pos] = plot_trajectory(x,u,ctrl_info,params) + fig_time = figure; + + % check if input is 3-dimensional + n_traj = size(x,3); + assert(size(u,3) == n_traj); + assert(size(ctrl_info,3) == n_traj); + + nx = params.model.nx; + assert(nx == size(x,1)); + + t = 0:params.model.TimeStep:params.model.TimeStep*params.model.HorizonLength; + + % plot + axes_time = cell(5,1); + axes_time{1} = subplot(5,1,1); + hold on; + + for i = 1:n_traj + plot(axes_time{1},t,x(1,:,i),'DisplayName',sprintf('x_%d',i)); + plot(axes_time{1},t,x(3,:,i),'DisplayName',sprintf('z_%d',i)); + end + legend('Location','EastOutside') + + % max position + x_max = params.constraints.MaxAbsPositionXZ; + plot(axes_time{1}, [t(1); t(end)],[x_max; x_max],'k--','HandleVisibility','off'); + plot(axes_time{1}, [t(1); t(end)],[-x_max; -x_max],'k--','HandleVisibility','off'); + ylabel('Position [Mm]') + + axes_time{2} = subplot(5,1,2); + hold on; + for i = 1:n_traj + plot(axes_time{2},t,x(2,:,i),'DisplayName',sprintf('y_%d',i)); + end + + % max position + y_max = params.constraints.MaxAbsPositionY; + plot(axes_time{2},[t(1); t(end)],[y_max; y_max],'k--','HandleVisibility','off'); + plot(axes_time{2},[t(1); t(end)],[-y_max; -y_max],'k--','HandleVisibility','off'); + + legend('Location','EastOutside') + ylabel('Position [Mm]') + + axes_time{3} = subplot(5,1,3); + hold on; + for i = 1:n_traj + plot(axes_time{3},t,x(4,:,i),'DisplayName',sprintf('v_{x%d}',i)); + plot(axes_time{3},t,x(5,:,i),'DisplayName',sprintf('v_{y%d}',i)); + plot(axes_time{3},t,x(6,:,i),'DisplayName',sprintf('v_{z%d}',i)); + end + + legend('Location','EastOutside') + ylabel('Velocity [km/s]') + + axes_time{4} = subplot(5,1,4); + hold on; + % append one input value for plotting + u(:,length(t),:) = u(:,length(t)-1,:); + for i = 1:n_traj + stairs(axes_time{4},t,u(1,:,i)','DisplayName',sprintf('u_{x%d}',i)); + stairs(axes_time{4},t,u(2,:,i)','DisplayName',sprintf('u_{y%d}',i)); + stairs(axes_time{4},t,u(3,:,i)','DisplayName',sprintf('u_{z%d}',i)); + end + + % max thrust + thrust_max = params.constraints.MaxAbsThrust; + plot([t(1); t(end)],[thrust_max; thrust_max],'k--','HandleVisibility','off'); + plot([t(1); t(end)],[-thrust_max; -thrust_max],'k--','HandleVisibility','off'); + legend('Location','EastOutside') + ylabel('Thrust [N]') + + % feasibility + axes_time{5} = subplot(5,1,5); + hold on; + for i = 1:n_traj + % append one input value for plotting + ctrl_feas = [ctrl_info(:,:,i).ctrl_feas]; + ctrl_feas(length(t)) = ctrl_feas(length(t) - 1); + stairs(axes_time{5},t,ctrl_feas','DisplayName',sprintf('feas_%d',i)); + end + legend('Location','EastOutside') + ylabel('Controller feasible [0/1]') + + % link axes + axes_time = [axes_time{:}]; + linkaxes(axes_time,'x'); + xlabel('Time [s]') + + % plot without time + % YX-plane + fig_pos = figure; + axes_pos = cell(2,1); + axes_pos{1} = subplot(2,1,1); + hold on + colormap('hsv') + + for i = 1:n_traj + plot(axes_pos{1},x(2,:,i),x(1,:,i)); + end + + axis equal + xlabel('Y-Position [m]') + ylabel('X-Position [Mm]') + + % YZ plane + axes_pos{2} = subplot(2,1,2); + hold on + for i = 1:n_traj + plot(axes_pos{2},x(2,:,i),x(3,:,i)) + end + axis equal + xlabel('Y-Position [Mm]') + ylabel('Z-Position [Mm]') + + axes_pos = [axes_pos{:}]; + linkaxes(axes_pos,'x'); +end \ No newline at end of file diff --git a/templates/plot_trajectory_z.m b/templates/plot_trajectory_z.m new file mode 100644 index 0000000..1b5a842 --- /dev/null +++ b/templates/plot_trajectory_z.m @@ -0,0 +1,78 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +function [fig_time,axes_time,fig_pos,axes_pos] = plot_trajectory_z(x,u,ctrl_info,params) + fig_time = figure; + + % check if input is 3-dimensional + n_traj = size(x,3); + assert(size(u,3) == n_traj); + assert(size(ctrl_info,3) == n_traj); + + nx = params.model.nx; + assert(nx == size(x,1)); + + t = 0:params.model.TimeStep:params.model.TimeStep*params.model.HorizonLength; + + % plot + axes_time = cell(4,1); + axes_time{1} = subplot(4,1,1); + hold on; + + for i = 1:n_traj + plot(axes_time{1},t,x(1,:,i),'DisplayName',sprintf('z_%d',i)); + end + legend('Location','EastOutside') + + % max position + x_max = params.constraints.MaxAbsPositionXZ; %*T(1); + plot(axes_time{1}, [t(1); t(end)],[x_max; x_max],'k--','HandleVisibility','off'); + plot(axes_time{1}, [t(1); t(end)],[-x_max; -x_max],'k--','HandleVisibility','off'); + ylabel('Position [Mm]') + + axes_time{2} = subplot(4,1,2); + hold on; + for i = 1:n_traj + plot(axes_time{2},t,x(2,:,i),'DisplayName',sprintf('v_{z%d}',i)); + end + legend('Location','EastOutside') + ylabel('Velocity [km/s]') + + axes_time{3} = subplot(4,1,3); + hold on; + % append one input value for plotting + u(:,length(t),:) = u(:,length(t)-1,:); + for i = 1:n_traj + stairs(axes_time{3},t,u(1,:,i)','DisplayName',sprintf('u_{z%d}',i)); + end + + % max thrust + thrust_max = params.constraints.MaxAbsThrust; + plot([t(1); t(end)],[thrust_max; thrust_max],'k--','HandleVisibility','off'); + plot([t(1); t(end)],[-thrust_max; -thrust_max],'k--','HandleVisibility','off'); + legend('Location','EastOutside') + ylabel('Thrust [N]') + + % feasibility + axes_time{4} = subplot(4,1,4); + hold on; + for i = 1:n_traj + % append one input value for plotting + ctrl_feas = [ctrl_info(:,:,i).ctrl_feas]; + ctrl_feas(length(t)) = ctrl_feas(length(t) - 1); + stairs(axes_time{4},t,ctrl_feas','DisplayName',sprintf('feas_%d',i)); + end + legend('Location','EastOutside') + ylabel('Feasible [0/1]') + + % link axes + axes_time = [axes_time{:}]; + linkaxes(axes_time,'x') + xlabel('Time [s]') +end \ No newline at end of file diff --git a/templates/simulate.m b/templates/simulate.m new file mode 100644 index 0000000..3138cd1 --- /dev/null +++ b/templates/simulate.m @@ -0,0 +1,14 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [Xt,Ut,ctrl_info] = simulate(x0, ctrl, params) + +% YOUR CODE HERE +% Hint: you can access the control command with ctrl.eval(x(:,i)) + +end \ No newline at end of file diff --git a/templates/simulate_uncertain.m b/templates/simulate_uncertain.m new file mode 100644 index 0000000..dc9df26 --- /dev/null +++ b/templates/simulate_uncertain.m @@ -0,0 +1,11 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [Xt,Ut,ctrl_info] = simulate_uncertain(x0, ctrl, Wt, params) + % YOUR CODE HERE +end \ No newline at end of file diff --git a/templates/traj_constraints.m b/templates/traj_constraints.m new file mode 100644 index 0000000..4b0a081 --- /dev/null +++ b/templates/traj_constraints.m @@ -0,0 +1,12 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(x,u,params) + % YOUR CODE HERE +end + diff --git a/templates/traj_cost.m b/templates/traj_cost.m new file mode 100644 index 0000000..7e0b443 --- /dev/null +++ b/templates/traj_cost.m @@ -0,0 +1,11 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function J_Nt = traj_cost(Xt,Ut,Q,R) + % YOUR CODE HERE +end \ No newline at end of file diff --git a/testing/.MATLABDriveTag b/testing/.MATLABDriveTag new file mode 100644 index 0000000..91a4f10 --- /dev/null +++ b/testing/.MATLABDriveTag @@ -0,0 +1 @@ +da537f63-b054-4058-9b41-6907e21d5531 \ No newline at end of file diff --git a/testing/TestFunction.p b/testing/TestFunction.p new file mode 100644 index 0000000..7bb108c Binary files /dev/null and b/testing/TestFunction.p differ diff --git a/testing/TestFunction_LQR.p b/testing/TestFunction_LQR.p new file mode 100644 index 0000000..955ba03 Binary files /dev/null and b/testing/TestFunction_LQR.p differ diff --git a/testing/TestFunction_MPC.p b/testing/TestFunction_MPC.p new file mode 100644 index 0000000..b802e23 Binary files /dev/null and b/testing/TestFunction_MPC.p differ diff --git a/testing/TestFunction_MPC_TE.p b/testing/TestFunction_MPC_TE.p new file mode 100644 index 0000000..d035c80 Binary files /dev/null and b/testing/TestFunction_MPC_TE.p differ diff --git a/testing/TestFunction_MPC_TE_forces.p b/testing/TestFunction_MPC_TE_forces.p new file mode 100644 index 0000000..81ff7ba Binary files /dev/null and b/testing/TestFunction_MPC_TE_forces.p differ diff --git a/testing/TestFunction_MPC_TS.p b/testing/TestFunction_MPC_TS.p new file mode 100644 index 0000000..23c4836 Binary files /dev/null and b/testing/TestFunction_MPC_TS.p differ diff --git a/testing/TestFunction_MPC_TS_SC.p b/testing/TestFunction_MPC_TS_SC.p new file mode 100644 index 0000000..faef7e4 Binary files /dev/null and b/testing/TestFunction_MPC_TS_SC.p differ diff --git a/testing/TestFunction_MPC_TS_SC_script.p b/testing/TestFunction_MPC_TS_SC_script.p new file mode 100644 index 0000000..e13715e Binary files /dev/null and b/testing/TestFunction_MPC_TS_SC_script.p differ diff --git a/testing/TestFunction_MPC_TUBE.p b/testing/TestFunction_MPC_TUBE.p new file mode 100644 index 0000000..4bac841 Binary files /dev/null and b/testing/TestFunction_MPC_TUBE.p differ diff --git a/testing/TestFunction_MPC_TUBE_script.p b/testing/TestFunction_MPC_TUBE_script.p new file mode 100644 index 0000000..9946b16 Binary files /dev/null and b/testing/TestFunction_MPC_TUBE_script.p differ diff --git a/testing/TestFunction_compute_minRPI.p b/testing/TestFunction_compute_minRPI.p new file mode 100644 index 0000000..9390f12 Binary files /dev/null and b/testing/TestFunction_compute_minRPI.p differ diff --git a/testing/TestFunction_compute_tightening.p b/testing/TestFunction_compute_tightening.p new file mode 100644 index 0000000..d2e60ea Binary files /dev/null and b/testing/TestFunction_compute_tightening.p differ diff --git a/testing/TestFunction_compute_tube_controller.p b/testing/TestFunction_compute_tube_controller.p new file mode 100644 index 0000000..764bd90 Binary files /dev/null and b/testing/TestFunction_compute_tube_controller.p differ diff --git a/testing/TestFunction_generate_constraints.p b/testing/TestFunction_generate_constraints.p new file mode 100644 index 0000000..6bff694 Binary files /dev/null and b/testing/TestFunction_generate_constraints.p differ diff --git a/testing/TestFunction_generate_disturbances.p b/testing/TestFunction_generate_disturbances.p new file mode 100644 index 0000000..9dc80b6 Binary files /dev/null and b/testing/TestFunction_generate_disturbances.p differ diff --git a/testing/TestFunction_generate_params.p b/testing/TestFunction_generate_params.p new file mode 100644 index 0000000..1ab5f90 Binary files /dev/null and b/testing/TestFunction_generate_params.p differ diff --git a/testing/TestFunction_generate_system.p b/testing/TestFunction_generate_system.p new file mode 100644 index 0000000..74e80ac Binary files /dev/null and b/testing/TestFunction_generate_system.p differ diff --git a/testing/TestFunction_generate_system_cont.p b/testing/TestFunction_generate_system_cont.p new file mode 100644 index 0000000..8c48d90 Binary files /dev/null and b/testing/TestFunction_generate_system_cont.p differ diff --git a/testing/TestFunction_generate_system_scaled.p b/testing/TestFunction_generate_system_scaled.p new file mode 100644 index 0000000..4176219 Binary files /dev/null and b/testing/TestFunction_generate_system_scaled.p differ diff --git a/testing/TestFunction_lqr_maxPI.p b/testing/TestFunction_lqr_maxPI.p new file mode 100644 index 0000000..a2fc282 Binary files /dev/null and b/testing/TestFunction_lqr_maxPI.p differ diff --git a/testing/TestFunction_lqr_tuning.p b/testing/TestFunction_lqr_tuning.p new file mode 100644 index 0000000..c97d218 Binary files /dev/null and b/testing/TestFunction_lqr_tuning.p differ diff --git a/testing/TestFunction_lqr_tuning_script.p b/testing/TestFunction_lqr_tuning_script.p new file mode 100644 index 0000000..09f6896 Binary files /dev/null and b/testing/TestFunction_lqr_tuning_script.p differ diff --git a/testing/TestFunction_simulate.p b/testing/TestFunction_simulate.p new file mode 100644 index 0000000..6751e46 Binary files /dev/null and b/testing/TestFunction_simulate.p differ diff --git a/testing/TestFunction_simulate_uncertain.p b/testing/TestFunction_simulate_uncertain.p new file mode 100644 index 0000000..9ec7139 Binary files /dev/null and b/testing/TestFunction_simulate_uncertain.p differ diff --git a/testing/TestFunction_traj_constraints.p b/testing/TestFunction_traj_constraints.p new file mode 100644 index 0000000..5805983 Binary files /dev/null and b/testing/TestFunction_traj_constraints.p differ diff --git a/testing/TestFunction_traj_cost.p b/testing/TestFunction_traj_cost.p new file mode 100644 index 0000000..d05dae0 Binary files /dev/null and b/testing/TestFunction_traj_cost.p differ diff --git a/testing/linearController.p b/testing/linearController.p new file mode 100644 index 0000000..ede2acc Binary files /dev/null and b/testing/linearController.p differ diff --git a/testing/run_tests.p b/testing/run_tests.p new file mode 100644 index 0000000..d233f5a Binary files /dev/null and b/testing/run_tests.p differ -- cgit v1.2.1