ADD: pass 9

1 files changed, 31 insertions, 1 deletions

diff --git a/templates/traj_constraints.m b/templates/traj_constraints.m
index d8ace29..d5e0c65 100644
--- a/templates/traj_constraints.m
+++ b/templates/traj_constraints.m
@@ -8,6 +8,36 @@
 
 function [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(x,u,params)
     % YOUR CODE HERE
-    s_max = 
+    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