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
|
