-
Notifications
You must be signed in to change notification settings - Fork 0
/
up.m
141 lines (125 loc) · 4.53 KB
/
up.m
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
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
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
function [h,Y] = up(kup,cup,bound,h,Y,THR);
% up actives one constraint and modifies consequently the HCOD.
%% Synopsis:
% [h Y] = up(kup,cup,bound,h,Y)
% [h Y] = up(kup,cup,bound,h,Y,THR)
% [h Y] = up(kup,cup,"right",h,Y)
% [h Y] = up(kup,cup,"left",h,Y)
%% Input:
% kup level where the constraint is going to be activated
% cup id of the constraint of level kup to be activated
% bound (among 1="left" for upper bound and 2="right" for lower bound).
% Bound to be activated.
% h "h" structure storing all the HQP data.
% Y right basis of the HCOD.
% THR epsilon threshold used to decide the rank-deficiency of a matrix.
%% Output:
% h,Y the function modifies the input h and Y and returns them.
%
% This function is described in the paper "Hierarchical quadratic
% programming", in Part II, Section 3.2.
%
% Copyright Nicolas Mansard -- LAAS/CNRS
% -- and Adrien Escande -- JRL/CNRS
% -- cf. LICENSE.txt
%
% --- DEFAULT ARGUMENTS --------------------------------------------------------
nin = nargin;
if nin==5
% Default argument for the HCOD threshold.
THR=1e-8;
nin=nin+1;
end
% ---------------------------------------------------------------------
addpath('utils');
p = length(h);
nh = size(Y,1);
if isa(bound,'char')
if strcmp(bound,'left') bound=1;
elseif strcmp(bound,'right') bound=2;
else print('Error with the bound arg.'); bound =1;
end
end
% --- Current stage ------------------------------------------------------------
hk=h(kup);
iw=hk.iw; im=hk.im; r=hk.r; n=hk.n; ra=hk.ra; rp=hk.rp; m=hk.m;
% Add a new row to H and a new dimension in W.
hk.iw = [iw hk.fw(1)];
iw = [iw hk.fw(1)];
hk.fw = hk.fw(2:end);
hk.im = [im hk.fm(1)];
im = [im hk.fm(1)];
hk.fm = hk.fm(2:end);
hk.H(im(end),:) = hk.A(cup,:)*Y;
hk.W(iw(end),:) = 0;
hk.W(:,im(end)) = 0;
hk.W(iw(end),im(end)) = 1;
hk.m = m+1;
m = m+1;
hk.active(end+1,1) = cup;
hk.activeb(end+1,1) = cup + (bound==2)*hk.mmax;
hk.bound(cup) = bound;
% 1.a Compute the rank of the new row, Eq (II-18).
% Find the first element of the tail of the row such that the norm of the
% tail is higher than the threshold: rup = max{ r, norm(ML(end,r:end))>THR }.
rup = nh+1-find( cumsum(flipdim( hk.H(im(end),:).^2, 2 )) > THR^2, 1 );
% 1.b modify the decomposition of level k.
if rup<=ra
% 1.b.TRUE (case II-3.2.2) The new row does not increase the rank of the level.
if rup>rp
% There is nonzero elements below L, Eq (II-19).
for i=rup-rp:-1:1
Wi = givens( hk.H(im(:),rp+i),n+i,m )';
hk.H(im,:) = Wi*hk.H(im,:);
hk.W(iw,im) = hk.W(iw,im)*Wi';
end
% else: There is only zeros below L, Eq (II-20): nothing to do.
end
% Switch H row.
hk.im = [im(end) im(1:end-1)];
hk.n = n+1;
h(kup)=hk; clear hk;
return;
end
% 1.b.FALSE (case II-3.2.3) The new row does increase the rank of the level.
% Nullify the tail of the row:
Yup = eye(nh);
for i=rup-1:-1:ra+1 % The loop corresponds to Eq (II-16).
Yi = givens(hk.H(im(end),:),i,i+1);
hk.H(im(end),:) = hk.H(im(end),:)*Yi;
Yup = Yup*Yi;
end
hk.r = r+1;
hk.ra = ra+1;
h(kup)=hk; clear hk;
clear iw im r n ra rp m;
% --- Propagation --------------------------------------------------------------
% If Yup exists, apply it to the above levels (sec. II-3.2.4)
for k=kup+1:p
massert( not(exist('hk')) ,'Error, hk should have been cleared.');
hk=h(k);
iw=hk.iw; im=hk.im; r=hk.r; n=hk.n; ra=hk.ra; rp=hk.rp; m=hk.m;
hk.H(im,:) = hk.H(im,:)*Yup;
if rup<rp+1
% (case II-3.2.7): Rank already lost, nothing to do.
elseif rup>ra
% (Case II-3.2.5): Rank lost later, L simply shifts on the right.
hk.rp = rp+1;
hk.ra = ra+1;
else
% (case II-3.2.6) Aup is colinear to one row of this level: rank lost here.
rdef = rup-rp; % Row to be removed from L.
for i=rdef:-1:2 % The loop corresponds to Eq (II-16).
Wi = givens(hk.H(im(:),rp+i),n+i-1,n+rdef)';
hk.H(im(:),:) = Wi*hk.H(im(:),:);
hk.W(iw,im) = hk.W(iw,im)*Wi';
end
hk.rp = rp+1;
hk.r = r-1;
hk.n = n+1;
hk.im = im( [ n+rdef 1:n n+1:n+rdef-1 n+rdef+1:m] );
end
h(k)=hk; clear hk;
clear iw im r n ra rp m;
end
Y=Y*Yup;