summaryrefslogtreecommitdiffstats
path: root/templates/traj_constraints.m
blob: c87bed6b5679e4850c328c15345d7f31426fb251 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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(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, ...
        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