summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorYanzhenXiangRobotics <xyz000327@gmail.com>2023-04-27 15:16:15 +0200
committerYanzhenXiangRobotics <xyz000327@gmail.com>2023-04-27 15:16:15 +0200
commitf2658db9daaa332db516bc42a5ddddbd124c03a8 (patch)
tree2df83c66779d960d6e01bfff43f7e421f048925e
parentADD: plot traj (diff)
downloadmpc_pe-f2658db9daaa332db516bc42a5ddddbd124c03a8.tar.gz
mpc_pe-f2658db9daaa332db516bc42a5ddddbd124c03a8.zip
ADD: pass 9
Diffstat (limited to '')
-rw-r--r--templates/traj_constraints.m32
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