From f2658db9daaa332db516bc42a5ddddbd124c03a8 Mon Sep 17 00:00:00 2001
From: YanzhenXiangRobotics <xyz000327@gmail.com>
Date: Thu, 27 Apr 2023 15:16:15 +0200
Subject: ADD: pass 9
---
templates/traj_constraints.m | 32 +++++++++++++++++++++++++++++++-
1 file changed, 31 insertions(+), 1 deletion(-)
(limited to 'templates')
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
--
cgit v1.2.1