%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 [H_u, h_u, H_x, h_x] = generate_constraints(params)
  H_u = [eye(params.model.nu); -eye(params.model.nu)];
  h_u = params.constraints.MaxAbsThrust * ones(params.model.nu*2,1);

  H_x = [eye(3), zeros(3); -eye(3), zeros(3)]; % * inv(params.model.ScalingMatrix);
  h_x = params.constraints.MaxAbsPositionXZ * [1, 0, 1, 1, 0, 1].' + ...
        params.constraints.MaxAbsPositionY * [0, 1, 0, 0, 1, 0].';
end