summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorYanzhenXiangRobotics <xyz000327@gmail.com>2023-05-01 21:36:59 +0200
committerYanzhenXiangRobotics <xyz000327@gmail.com>2023-05-01 21:36:59 +0200
commit2be9f8ec42ca8bcd765804c070e246f6f5e4805d (patch)
tree6f8155e85ae613712b8bc71ab285391fbf0fc76e
parentADD: pass 9 (diff)
downloadmpc_pe-2be9f8ec42ca8bcd765804c070e246f6f5e4805d.tar.gz
mpc_pe-2be9f8ec42ca8bcd765804c070e246f6f5e4805d.zip
ADD: not using traj_constr version (not correct)
-rw-r--r--templates/lqr_tuning.asv50
-rw-r--r--templates/lqr_tuning.m42
2 files changed, 92 insertions, 0 deletions
diff --git a/templates/lqr_tuning.asv b/templates/lqr_tuning.asv
new file mode 100644
index 0000000..71c92ef
--- /dev/null
+++ b/templates/lqr_tuning.asv
@@ -0,0 +1,50 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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
+ R = eye(params.model.nu);
+ size_Q = size(Q);
+ param_choices = size_Q(2);
+ J_array = [];
+ for i = 1:param_choices
+ Q_mat = diag(Q(:,i));
+ ctrl = LQR(Q_mat,R,params);
+ [Xt, Ut, ctrl_info] = simulate(x0, ctrl, params);
+ ctrl_feas_arr = [];
+ for k = 1:params.model.HorizonLength
+ ctrl_feas_arr = [ctrl_feas_arr,ctrl_info(k).ctrl_feas];
+ end
+% display(ctrl_feas_arr);
+ 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];
+ end
+ end
+% display(J_array)
+ if isempty(J_array)
+ i_opt = nan;
+ tuning_struct.TrajFeasible = false;
+ else
+ [~, i_opt] = min(J_array);
+ end
+ tuning_struct.TrajFeasible = true;
+ tuning_struct.InitialCondition = x0;
+ tuning_struct.Qdiag = Q(:,i_opt);
+ tuning_struct.MaxAbsPositionXZ = params.constraints.MaxAbsPositionXZ;
+ tuning_struct.MaxAbsPositionY = params.constraints.MaxAbsPositionY;
+ tuning_struct.MaxAbsThrust = params.constraints.MaxAbsThrust;
+ tuning_struct.InputCost = J_
+ tuning_struct.MaxFinalPosDiff = params.constraints.MaxFinalPosDiff;
+ tuning_struct.MaxFinalVelDiff = params.constraints.MaxFinalVelDiff;
+end \ No newline at end of file
diff --git a/templates/lqr_tuning.m b/templates/lqr_tuning.m
index 4a26158..8e6edc1 100644
--- a/templates/lqr_tuning.m
+++ b/templates/lqr_tuning.m
@@ -8,4 +8,46 @@
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, ctrl_info] = simulate(x0, ctrl, params);
+ 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 = params.constraints.MaxAbsPositionXZ;
+ tuning_struct_item.MaxAbsPositionY = params.constraints.MaxAbsPositionY;
+ tuning_struct_item.MaxAbsThrust = params.constraints.MaxAbsThrust;
+ tuning_struct_item.InputCost = J_array(i);
+ tuning_struct_item.MaxFinalPosDiff = params.constraints.MaxFinalPosDiff;
+ tuning_struct_item.MaxFinalVelDiff = params.constraints.MaxFinalVelDiff;
+ 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