ADD; pass 10

1 files changed, 31 insertions, 23 deletions

diff --git a/templates/lqr_tuning.m b/templates/lqr_tuning.m
index 8e6edc1..dc7161b 100644
--- a/templates/lqr_tuning.m
+++ b/templates/lqr_tuning.m
@@ -16,31 +16,39 @@ function [tuning_struct, i_opt] = lqr_tuning(x0,Q,params)
     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
+        [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 = 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_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)