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
40
41
42
43
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
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
|
