diff options
author | Yuan Xu <yuanxu@student.ethz.ch> | 2023-05-24 17:35:46 +0200 |
---|---|---|
committer | Yuan Xu <yuanxu@student.ethz.ch> | 2023-05-24 17:35:46 +0200 |
commit | a23ba49056a36aff221d0c23e228b981222c3576 (patch) | |
tree | 742c18cbe13c1603909b55ee82188ce6360e8fa0 /Submission files/traj_constraints.m | |
parent | ADD: declaration of originality (diff) | |
download | mpc_pe-a23ba49056a36aff221d0c23e228b981222c3576.tar.gz mpc_pe-a23ba49056a36aff221d0c23e228b981222c3576.zip |
add submission zipsubmission
Diffstat (limited to '')
-rw-r--r-- | Submission files/traj_constraints.m | 40 |
1 files changed, 40 insertions, 0 deletions
diff --git a/Submission files/traj_constraints.m b/Submission files/traj_constraints.m new file mode 100644 index 0000000..69d19d3 --- /dev/null +++ b/Submission files/traj_constraints.m @@ -0,0 +1,40 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich. +% +% All rights reserved. +% +% Please see the LICENSE file that has been included as part of this package. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +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 = max(max(abs(x([1,3],:)))); + y_max = max(abs(x(2,:))); + u_max = max(max(abs(u))); + + J_u = 0; + for k=1:size(u,2) + 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)); + + constr = [ + s_max <= params.constraints.MaxAbsPositionXZ, ... + y_max <= params.constraints.MaxAbsPositionY, ... + u_max <= params.constraints.MaxAbsThrust, ... + df_max <= params.constraints.MaxFinalPosDiff, ... + vf_max <= params.constraints.MaxFinalVelDiff + ]; + + % disp([s_max, y_max, df_max, vf_max]) + % disp([params.constraints.MaxAbsPositionXZ, ... + % params.constraints.MaxAbsPositionY, ... + % params.constraints.MaxFinalPosDiff, ... + % params.constraints.MaxFinalVelDiff]) + % disp(constr') + traj_feas = all(constr); +end + |