VVIEW - View the attainable virtual control set.
1) vview(B,plim)
Shows the attainable virtual control set considering actuator
position constraints, given by { v : v = B*u, umin < u < umax }.
2) ratio = vview(B,plim,P)
Compares the set of feasible virtual control inputs when
a) the actuator redundancy is fully utilized (as above) [blue]
b) a linear allocation control law u = Pv is used (BP = I) [red]
The second set is given by { v : umin < P*v < umax }.
Inputs:
-------
B control effectiveness matrix (k x m)
plim control position limits [min max] (m x 2)
P virtual control law matrix (m x k)
Outputs:
--------
ratio The ratio between the sizes (areas, volumes, ...)
of the two sets
The result is only graphically illustrated for k = 1, 2, or 3.
See also: VVIEW_DEMO0001 function ratio = vview(B,plim,P) 0002 0003 % VVIEW - View the attainable virtual control set. 0004 % 0005 % 1) vview(B,plim) 0006 % 0007 % Shows the attainable virtual control set considering actuator 0008 % position constraints, given by { v : v = B*u, umin < u < umax }. 0009 % 0010 % 2) ratio = vview(B,plim,P) 0011 % 0012 % Compares the set of feasible virtual control inputs when 0013 % 0014 % a) the actuator redundancy is fully utilized (as above) [blue] 0015 % b) a linear allocation control law u = Pv is used (BP = I) [red] 0016 % 0017 % The second set is given by { v : umin < P*v < umax }. 0018 % 0019 % Inputs: 0020 % ------- 0021 % B control effectiveness matrix (k x m) 0022 % plim control position limits [min max] (m x 2) 0023 % P virtual control law matrix (m x k) 0024 % 0025 % Outputs: 0026 % -------- 0027 % ratio The ratio between the sizes (areas, volumes, ...) 0028 % of the two sets 0029 % 0030 % The result is only graphically illustrated for k = 1, 2, or 3. 0031 % 0032 % See also: VVIEW_DEMO 0033 0034 % Model dimensions 0035 [k,m] = size(B); 0036 0037 % ------------------------------------------------ 0038 % a) Find maximum attainable virtual control set 0039 % considering constraints. 0040 % ------------------------------------------------ 0041 0042 % Generate matrix to index corners of feasible control set. 0043 idx = zeros(2^m,m); 0044 M = 1:m; 0045 for i = 1:2^m; 0046 cbin = dec2bin(i-1,m); % '001' 0047 c = str2num(cbin')'; % [0 0 1] 0048 c = c(end:-1:1); % [1 0 0] 0049 idx(i,:) = 2*M - c; 0050 end 0051 0052 % Generate corner points of the feasible control set. 0053 plimT = plim'; 0054 U = plimT(idx)'; 0055 0056 % Compute the corresponding points in the virtual control space 0057 V = B*U; 0058 0059 if nargin > 2 0060 0061 % --------------------------------------------- 0062 % b) Find attainable virtual control set when 0063 % a linear control law u=Pv is used. 0064 % --------------------------------------------- 0065 0066 % We want to determine where the k-dim. hyperplane Pv 0067 % intersects the m-dim. hyperbox of feasible controls. 0068 % To get the corner points of this set, solve 0069 % Pv = x where x has k specified entries. 0070 % 0071 % Example: m=3, k=1 -> points will lie on surfaces 0072 % m=3, k=2 -> points will lie on edges 0073 0074 % Generate index matrix for all combinations of min and max indeces 0075 % in k dimensions. 0076 sub_idx = idx(1:2^k,1:k); 0077 0078 Ulin = []; 0079 % Loop over all combinations of dimensions 0080 i_dim = nchoosek(1:m,k); 0081 for i = 1:size(i_dim,1) 0082 % For each combination, compute the intersections with all 0083 % possible min/max combinations. 0084 0085 % k-dimensional min/max combinations 0086 sub_plimT = plimT(:,i_dim(i,:)); 0087 sub_u_boundary = sub_plimT(sub_idx)'; 0088 0089 % Determine which virtual control sub_u_boundary corresponds to 0090 sub_P = P(i_dim(i,:),:); 0091 if rank(sub_P) == k % Avoid "parallel" cases 0092 % Solve sub_u_boundary = sub_P v for v 0093 v = sub_P\sub_u_boundary; 0094 % Determine the full countol vector (contains sub_u_boundary) 0095 u_boundary = P*v; 0096 0097 % Store feasible points 0098 i_feas = feasible(u_boundary,plim); 0099 Ulin = [Ulin u_boundary(:,i_feas)]; 0100 end 0101 end 0102 0103 % Compute the corresponing points in the virtual control space 0104 Vlin = B*Ulin; 0105 0106 end 0107 0108 % Compute and visualize the convex hull of the set(s) 0109 clf 0110 switch k 0111 case 1 0112 K = [min(V) max(V)]; 0113 if nargin > 2 0114 Klin = [min(Vlin) max(Vlin)]; 0115 ratio = diff(Klin)/diff(K); 0116 0117 % Illustrate 0118 plot(K,[0 0],'b-o',Klin,-[0 0],'r-o') 0119 else 0120 plot(K,[0 0],'b-o') 0121 end 0122 xlabel('v') 0123 0124 case 2 0125 [K,area1] = convhull(V(1,:),V(2,:)); 0126 if nargin > 2 0127 [Klin,area2] = convhull(Vlin(1,:),Vlin(2,:)); 0128 ratio = area2/area1; 0129 0130 % Illustrate 0131 fill(V(1,K),V(2,K),[.95 .95 1],... 0132 Vlin(1,Klin),Vlin(2,Klin),[1 1 .9]) 0133 hold on; 0134 plot(Vlin(1,Klin),Vlin(2,Klin),'r',V(1,K),V(2,K),'b') 0135 hold off; 0136 else 0137 fill(V(1,K),V(2,K),[.95 .95 1]); 0138 hold on; 0139 plot(V(1,K),V(2,K),'b') 0140 hold off; 0141 end 0142 axis equal; 0143 xlabel('v_1') 0144 ylabel('v_2') 0145 0146 otherwise 0147 [K,vol1] = convhulln(V'); 0148 if nargin > 2 0149 [Klin,vol2] = convhulln(Vlin'); 0150 ratio = vol2/vol1; 0151 end 0152 0153 if k == 3 0154 % Illustrate 0155 if nargin > 2 0156 h = polyplot(Klin,Vlin',1); 0157 set(h,'EdgeColor','r','FaceColor',[1 1 .9]); 0158 hold on; 0159 % Fix: Make V wireframe enclose Vlin 0160 V0 = repmat(mean(V')',1,size(V,2)); 0161 V = 1.0001*(V-V0)+V0; 0162 h = polyplot(K,V',1); 0163 set(h,'EdgeColor',[.6 .6 1],'FaceColor','none'); 0164 hold off 0165 else 0166 h = polyplot(K,V',1); 0167 set(h,'EdgeColor','b','FaceColor',[.95 .95 1]); 0168 end 0169 0170 xlabel('v_1') 0171 ylabel('v_2') 0172 zlabel('v_3') 0173 view(3); 0174 axis equal; 0175 axis vis3d; 0176 grid on; 0177 end 0178 end 0179 0180 function f = feasible(x,plim) 0181 % x m*n 0182 % lb m 0183 % ub m 0184 0185 m = size(x,1); 0186 0187 % Mean point 0188 x0 = mean(plim,2); 0189 0190 % Make the mean point the origin 0191 x = x - x0*ones(1,size(x,2)); 0192 lb = plim(:,1) - x0; % < 0 0193 ub = plim(:,2) - x0; % > 0 0194 0195 % Check for feasibility 0196 tol = 1e-5; 0197 f = sum((diag(1./ub)*x <= 1+tol) & (diag(1./lb)*x <= 1+tol)) == m; 0198 0199 function h = polyplot(face,vert,merge) 0200 0201 if merge 0202 % Merge adjacent, parallel triangles to get fewer lines that 0203 % are not edges of the polyhedron. 0204 face4 = []; 0205 % Loop over all combinations of triangles 0206 k = 1; 0207 while k < size(face,1) 0208 l = k+1; 0209 while l <= size(face,1) 0210 iv = intersect(face(k,:),face(l,:)); % Intersecting vertices 0211 if length(iv) == 2 % Two common vertices 0212 % Are the faces parallel? 0213 niv = setxor(face(k,:),face(l,:)); % Non-intersecting vertices 0214 % Vectors from first common vertex to remaining three vertices 0215 A = [vert(iv(2),:) - vert(iv(1),:); 0216 vert(niv(1),:) - vert(iv(1),:); 0217 vert(niv(2),:) - vert(iv(1),:)]; 0218 if abs(det(A))<100*eps 0219 % Vectors lie in same plane -> create patch with four vertices 0220 face4 = [face4 ; iv(1) niv(1) iv(2) niv(2)]; 0221 % ... and remove the two triangles 0222 face = face([1:k-1 k+1:l-1 l+1:end],:); 0223 k = k-1; 0224 break 0225 end 0226 end 0227 l = l+1; 0228 end % inner loop 0229 k = k+1; 0230 end % outer loop 0231 h = [patch('Faces',face,'Vertices',vert) 0232 patch('Faces',face4,'Vertices',vert)]; 0233 else 0234 % Just plot the polyhedron made up by triangles 0235 h = patch('Faces',face,'Vertices',vert); 0236 end 0237 0238