From 3d6ef58ff1edee6d882a8d9cbe97625ff26afe6e Mon Sep 17 00:00:00 2001 From: YanzhenXiangRobotics Date: Wed, 10 May 2023 23:30:01 +0200 Subject: Revert to handout --- templates/LQR.m | 7 --- templates/MPC.m | 89 ++++---------------------------------- templates/dummy_test.m | 0 templates/generate_constraints.m | 14 ------ templates/generate_params.m | 28 ------------ templates/generate_system.m | 8 ---- templates/generate_system_cont.m | 11 ----- templates/generate_system_scaled.m | 4 -- templates/lqr_maxPI.m | 70 +----------------------------- templates/lqr_tuning.m | 50 --------------------- templates/simulate.m | 15 ------- templates/traj_constraints.m | 31 ------------- templates/traj_cost.m | 6 --- 13 files changed, 11 insertions(+), 322 deletions(-) delete mode 100644 templates/dummy_test.m (limited to 'templates') diff --git a/templates/LQR.m b/templates/LQR.m index c05237d..2a79da9 100644 --- a/templates/LQR.m +++ b/templates/LQR.m @@ -16,18 +16,11 @@ classdef LQR function obj = LQR(Q,R,params) % YOUR CODE HERE % obj.K = ... (save feedback matrix for use in eval function) - A = params.model.A; - B = params.model.B; - % [X, L, G] = dare(A,B,Q,R); - [K,S,e] = dlqr(A,B,Q,R) - % obj.K = -inv(B'*P_inf*B+R)*B'*P_inf*A; - obj.K = -K; end function [u, ctrl_info] = eval(obj,x) % YOUR CODE HERE % u = ... - u = obj.K*x; ctrl_info = struct('ctrl_feas',true); end end diff --git a/templates/MPC.m b/templates/MPC.m index b2d411d..3e9d2f1 100644 --- a/templates/MPC.m +++ b/templates/MPC.m @@ -6,44 +6,6 @@ % 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 - classdef MPC properties yalmip_optimizer @@ -53,64 +15,31 @@ classdef MPC function obj = MPC(Q,R,N,params) nu = params.model.nu; nx = params.model.nx; - % YOUR CODE HERE + % define optimization variables - A=params.model.A; - B=params.model.B; U = sdpvar(repmat(nu,1,N),ones(1,N),'full'); - X = sdpvar(repmat(nx,1,N+1),ones(1,N+1),'full'); - - [K,P,~] = dlqr(A,B,Q,R); - - % define constraints -% s_max=params.constraints.MaxAbsPositionXZ; -% y_max=params.constraints.MaxAbsPositionY; -% u_max = params.constraints.MaxAbsThrust; - H_x = params.constraints.StateMatrix; - h_x = params.constraints.StateRHS; - H_u = params.constraints.InputMatrix; - h_u = params.constraints.InputRHS; X0 = sdpvar(nx,1,'full'); - objective = 0; - constraints = X{1} == X0; - for k = 1:N - constraints = [ ... - constraints, ... - X{k+1} == A*X{k} + B*U{k} , ... - H_x * X{k} <= h_x, ... - H_u * U{k} <= h_u ... - ]; - objective = objective + X{k}'*Q*X{k} + U{k}'*R*U{k}; - end - objective=objective+X{N+1}'*P*X{N+1}; - % terminal constraint -% constraints = [ ... -% constraints, ... -% X{N+1} == zeros(nx,1) -% ]; + % 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 + %% evaluate control action by solving MPC problem, e.g. tic; - [optimizer_out,errorcode,~] = obj.yalmip_optimizer{x}; + [optimizer_out,errorcode] = obj.yalmip_optimizer(x); solvetime = toc; + + [u, objective] = optimizer_out{:}; - % extract optimal control action and objective function value - u = optimizer_out{1}; - objective = optimizer_out{2}; - - % check feasibility of optimization problem - feasible = ~isnan(objective) && ~isinf(objective); + feasible = true; if (errorcode ~= 0) feasible = false; end - % create control info struct ctrl_info = struct('ctrl_feas',feasible,'objective',objective,'solvetime',solvetime); end end -end +end \ No newline at end of file diff --git a/templates/dummy_test.m b/templates/dummy_test.m deleted file mode 100644 index e69de29..0000000 diff --git a/templates/generate_constraints.m b/templates/generate_constraints.m index 008f50c..892b706 100644 --- a/templates/generate_constraints.m +++ b/templates/generate_constraints.m @@ -8,18 +8,4 @@ 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)]; - H_x=[1,0,0,0,0,0; - 0,1,0,0,0,0; - 0,0,1,0,0,0; - -1,0,0,0,0,0; - 0,-1,0,0,0,0; - 0,0,-1,0,0,0]; end \ No newline at end of file diff --git a/templates/generate_params.m b/templates/generate_params.m index e2e6024..ef51366 100644 --- a/templates/generate_params.m +++ b/templates/generate_params.m @@ -41,33 +41,5 @@ 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 c4dadc2..9deb347 100644 --- a/templates/generate_system.m +++ b/templates/generate_system.m @@ -8,12 +8,4 @@ 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 770fd8e..2d3ee79 100644 --- a/templates/generate_system_cont.m +++ b/templates/generate_system_cont.m @@ -8,15 +8,4 @@ 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 38714d2..eac8db8 100644 --- a/templates/generate_system_scaled.m +++ b/templates/generate_system_scaled.m @@ -8,8 +8,4 @@ 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 diff --git a/templates/lqr_maxPI.m b/templates/lqr_maxPI.m index 47ddd37..efa335a 100644 --- a/templates/lqr_maxPI.m +++ b/templates/lqr_maxPI.m @@ -6,73 +6,7 @@ % Please see the LICENSE file that has been included as part of this package. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% function [H, h] = lqr_maxPI(Q, R, params) -% % Define the linear system -% A = params.model.A; -% B = params.model.B; -% -% % Define the state and input constraints -% s_max=params.constraints.MaxAbsPositionXZ; -% y_max=params.constraints.MaxAbsPositionY; -% u_max = params.constraints.MaxAbsThrust; -% -% % Define the cost function -% [K, ~, ~] = dlqr(A, B, Q, R); -% P = dare(A, B, Q, R) -% F = -inv((B'*P*B+R))*B'*P*A -% A -% B -% Ax=[1,0; -% 0,1; -% -1,0; -% 0,-1]; -% Ax=[1,0; -% -1,0; -% 0,1; -% 0,-1]; -% Au=Ax; -% bx=[s_max;s_max;s_max;s_max]; -% bu=[u_max;u_max;u_max;u_max]; -% H=[Ax;Au*F]/2 -% h=[bx;bu] -% end - -function [H, h] = lqr_maxPI(Q, R, params) - % Define the linear system - A = params.model.A; - B = params.model.B; - nx=params.model.nx; - nu=params.model.nu; - - % Define the state and input constraints - s_max= params.constraints.MaxAbsPositionXZ; - y_max= params.constraints.MaxAbsPositionY; - u_max = params.constraints.MaxAbsThrust; - - % Define the cost function - K = -dlqr(A, B, Q, R); - systemLQR = LTISystem('A', A+B*K); -% absxmax = ones(nx,1)*s_max; -% absumax = ones(nu,1)*u_max; -% Xp = Polyhedron('A',[eye(nx); -eye(nx); K; -K], 'b', [absxmax;absxmax; absumax;absumax]); - Hx=params.constraints.StateMatrix; - hx=params.constraints.StateRHS; - Hu=params.constraints.InputMatrix; - hu=params.constraints.InputRHS; - Xp = Polyhedron('A',[Hx;Hu*K], 'b', [hx;hu]); - - figure(1); -% Xp.plot(), alpha(0.25), title('Resulting State Constraints under LQR Control'), xlabel('x_1'), ylabel('x_2'), zlabel('x_3'); - systemLQR.x.with('setConstraint'); - systemLQR.x.setConstraint = Xp; - InvSetLQR = systemLQR.invariantSet(); - InvSetLQR.plot(), alpha(0.25), title('Invariant Set for Triple Integrator under LQR Control'), xlabel('x_1'), ylabel('x_2'), zlabel('x_3'); - - H=InvSetLQR.A - h=InvSetLQR.b - - +function [H, h] = lqr_maxPI(Q,R,params) + % YOUR CODE HERE end - - diff --git a/templates/lqr_tuning.m b/templates/lqr_tuning.m index dc7161b..4a26158 100644 --- a/templates/lqr_tuning.m +++ b/templates/lqr_tuning.m @@ -8,54 +8,4 @@ function [tuning_struct, i_opt] = lqr_tuning(x0,Q,params) % YOUR CODE HERE - R = eye(params.model.nu); - size_Q = size(Q); - param_choices = size_Q(2); - J_array = []; - tuning_struct = []; - for i = 1:param_choices - Q_mat = diag(Q(:,i)); - ctrl = LQR(Q_mat,R,params); - [Xt, Ut, ~] = simulate(x0, ctrl, params); - [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(Xt,Ut,params); - % YOUR CODE HERE - % ctrl_feas_arr = []; - % for k = 1:params.model.HorizonLength - % ctrl_feas_arr = [ctrl_feas_arr,ctrl_info(k).ctrl_feas]; - % end - % if all(ctrl_feas_arr) - % J = 0; - % for j = 1:params.model.HorizonLength - % J = J + Xt(:,j)'*Q_mat*Xt(:,j); - % J = J + Ut(:,j)'*R*Ut(:,j); - % end - % J = J + Xt(:,end)'*Q_mat*Xt(:,end); - % J_array = [J_array, J]; - % tuning_struct_item.TrajFeasible = true; - % else - % tuning_struct_item.TrajFeasible = false; - % end - tuning_struct_item.InitialCondition = x0; - tuning_struct_item.Qdiag = Q(:,i); - tuning_struct_item.MaxAbsPositionXZ = s_max; - tuning_struct_item.MaxAbsPositionY = y_max; - tuning_struct_item.MaxAbsThrust = u_max; - tuning_struct_item.InputCost = J_u; - tuning_struct_item.MaxFinalPosDiff = df_max; - tuning_struct_item.MaxFinalVelDiff = vf_max; - tuning_struct_item.TrajFeasible = traj_feas; - - if traj_feas - J_array = [J_array, J_u]; - end - - tuning_struct = [tuning_struct,tuning_struct_item]; - end -% display(J_array) - tuning_struct = tuning_struct'; - if isempty(J_array) - i_opt = nan; - else - [~, i_opt] = min(J_array); - end end \ No newline at end of file diff --git a/templates/simulate.m b/templates/simulate.m index 6100434..3138cd1 100644 --- a/templates/simulate.m +++ b/templates/simulate.m @@ -10,20 +10,5 @@ function [Xt,Ut,ctrl_info] = simulate(x0, ctrl, params) % YOUR CODE HERE % Hint: you can access the control command with ctrl.eval(x(:,i)) - Xt = [x0]; - Ut = []; - x = x0 - ctrl_info = []; - % obj = LQR(); - for i = 1:params.model.HorizonLength - [u, ctrl_info_] = ctrl.eval(x); - Ut = [Ut u]; - ctrl_info = [ctrl_info ctrl_info_]; - x = params.model.A*x + params.model.B*u; - Xt = [Xt x]; - end - plot_trajectory(Xt, Ut, ctrl_info, params); -% Ut = u; -% Xt = x; end \ No newline at end of file diff --git a/templates/traj_constraints.m b/templates/traj_constraints.m index d5e0c65..4b0a081 100644 --- a/templates/traj_constraints.m +++ b/templates/traj_constraints.m @@ -8,36 +8,5 @@ function [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(x,u,params) % YOUR CODE HERE - traj_feas = true; - % timesteps = params.model.TimeStep; - size_u = size(u); - timesteps = size_u(end); - s_max = max(max(abs(x(1,:))),max(abs(x(3,:)))); - y_max = max(abs(x(2,:))); - u_max = max(abs(u)); - J_u = 0; - % fprintf("timestep: %s\n\n\n",timesteps); -% fprintf("u shape: %s\n\n\n",size(u)); - % display(size(x)) - for k = 1:timesteps - J_u = J_u + u(:,k)'*u(:,k); - end - df_max = sqrt(sum(x(1:3,end).^2)); - vf_max = sqrt(sum(x(4:6,end).^2)); - if s_max > params.constraints.MaxAbsPositionXZ - traj_feas = false; - end - if y_max > params.constraints.MaxAbsPositionY - traj_feas = false; - end - if u_max > params.constraints.MaxAbsThrust - traj_feas = false; - end - if df_max > params.constraints.MaxFinalPosDiff - traj_feas = false; - end - if vf_max > params.constraints.MaxFinalVelDiff - traj_feas = false; - end end diff --git a/templates/traj_cost.m b/templates/traj_cost.m index 57566c9..7e0b443 100644 --- a/templates/traj_cost.m +++ b/templates/traj_cost.m @@ -8,10 +8,4 @@ function J_Nt = traj_cost(Xt,Ut,Q,R) % YOUR CODE HERE - J_Nt=0; - for i=1:length(Xt(1,:))-1 - a= Xt(:,i)'*Q*Xt(:,i); - b= Ut(:,i)'*R*Ut(:,i); - J_Nt=J_Nt+a+b; - end end \ No newline at end of file -- cgit v1.2.1