1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
|
function [Gamma,gamma] = ppp_input_constraint (A_u,Tau,Min,Max,Order,i_u,n_u)
## usage: [Gamma,gamma] = ppp_input_constraint (A_u,Tau,Min,Max,Order)
##
## Derives the input constraint matrices Gamma and gamma
## For Constraints Min and max at times Tau
## Order=0 - input constraints
## Order=1 - input derivative constraints
## etc
## i_u: Integer index of the input to be constrained
## n_u: Number of inputs
## NOTE You can stack up Gamma and gamma matrices for create multi-input constraints.
## Copyright (C) 1999 by Peter J. Gawthrop
## $Id$
## Sizes
[n_U,m_U] = size(A_u); # Number of basis functions
|
|
|
>
>
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
|
function [Gamma,gamma] = ppp_input_constraint (A_u,Tau,Min,Max,Order,i_u,n_u)
## usage: [Gamma,gamma] = ppp_input_constraint (A_u,Tau,Min,Max,Order,i_u,n_u)
##
## Derives the input constraint matrices Gamma and gamma
## For Constraints Min and max at times Tau
## Order=0 - input constraints
## Order=1 - input derivative constraints
## etc
## i_u: Integer index of the input to be constrained
## n_u: Number of inputs
## NOTE You can stack up Gamma and gamma matrices for create
## multi-input constraints.
## Limits at inf and -inf are discarded
## Copyright (C) 1999 by Peter J. Gawthrop
## $Id$
## Sizes
[n_U,m_U] = size(A_u); # Number of basis functions
|
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
|
endif
n = length(Min);
m = length(Max);
o = length(Order);
if (n != N_t)||(m != N_t)||(o != N_t)
error("Tau, Min and max must be the same length");
endif
## Extract the A_i matrix for this input
A_i = ppp_extract(A_u,i_u);
## Create the constraints in the form: Gamma*U < gamma
Gamma = [];
gamma = [];
one = ones(m_U,1);
i=0;
zero_l = zeros(1,(i_u-1)*m_U); # Pad left-hand
zero_r = zeros(1,(n_u-i_u)*m_U); # Pad right-hand
for tau = Tau # Stack constraints for each tau
i++;
Gamma_tau = ( A_i^Order(i) * expm(A_i*tau) * one )';
Gamma_tau = [ zero_l Gamma_tau zero_r ]; # Only for i_uth input
Gamma = [Gamma;[-1;1]*Gamma_tau]; # One row for each of min and max
gamma_tau = [-Min(i);Max(i)];
gamma = [gamma;gamma_tau];
endfor
endfunction
|
|
>
>
>
|
>
>
|
|
>
|
>
>
|
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
|
endif
n = length(Min);
m = length(Max);
o = length(Order);
if (n != N_t)||(m != N_t)||(o != N_t)
error("Tau, Min, Max and Order must be the same length");
endif
## Extract the A_i matrix for this input
A_i = ppp_extract(A_u,i_u);
## Create the constraints in the form: Gamma*U < gamma
Gamma = [];
gamma = [];
one = ones(m_U,1);
i=0;
zero_l = zeros(1,(i_u-1)*m_U); # Pad left-hand
zero_r = zeros(1,(n_u-i_u)*m_U); # Pad right-hand
for tau = Tau # Stack constraints for each tau
i++;
Gamma_tau = ( A_i^Order(i) * expm(A_i*tau) * one )';
Gamma_tau = [ zero_l Gamma_tau zero_r ]; # Only for i_uth input
if Max(i)<inf
Gamma = [Gamma; Gamma_tau];
gamma = [gamma; Max(i)];
endif
if Min(i)>-inf
Gamma = [Gamma; -Gamma_tau];
gamma = [gamma; -Min(i)];
endif
endfor
endfunction
|