diff options
| author | Nao Pross <np@0hm.ch> | 2023-05-24 16:22:02 +0200 |
|---|---|---|
| committer | Nao Pross <np@0hm.ch> | 2023-05-24 16:32:02 +0200 |
| commit | b6f865025eed2db716f2f853435855edb0db9e82 (patch) | |
| tree | 0211e0d07543dcea8e8e6048ae58a3458f5ab2d9 /templates/traj_constraints.m | |
| parent | Take deliverables for system modelling from npross (diff) | |
| download | mpc_pe-b6f865025eed2db716f2f853435855edb0db9e82.tar.gz mpc_pe-b6f865025eed2db716f2f853435855edb0db9e82.zip | |
Take deliverables for uncontrained optimal control from npross
According to table 5 they are:
- LQR
- LQR/eval (contained in LQR.m)
- simulate
- traj_contraints
- lqr_tuning
- lqr_tuning_script (.m and its .mat output file)
Diffstat (limited to '')
| -rw-r--r-- | templates/traj_constraints.m | 28 |
1 files changed, 28 insertions, 0 deletions
diff --git a/templates/traj_constraints.m b/templates/traj_constraints.m index 4b0a081..69d19d3 100644 --- a/templates/traj_constraints.m +++ b/templates/traj_constraints.m @@ -8,5 +8,33 @@ 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 |