diff options
| -rw-r--r-- | templates/traj_constraints.m | 32 |
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 |