-
Notifications
You must be signed in to change notification settings - Fork 1
/
createWhitelist.m
executable file
·127 lines (121 loc) · 5.55 KB
/
createWhitelist.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
function [ opt_pars, min_pars, max_pars, whitelist, dh_pars] = createWhitelist( robot, dh_pars, lb_pars, ub_pars, optim, chains, linkTypes, funcname )
%CREATEWHITELIST selects whitelist and returns selected parameters based
% on the whitelist, together with lower/upper bounds for the
% parameters.
% INPUT - dh_pars - structure where fields are names of 'groups', each
% field is 4D array with kinematics parameters
% - lb_pars - structure where fields are names of 'groups', each
% field is 4D array with lower bounds
% - ub_pars - structure where fields are names of 'groups', each
% field is 4D array with upper bounds
% - optim - structure of calibration settings
% - chains - structure of chain settings
% - linkTypes - structure of 'types' settings
% - funcname - name of the robot-specific function
% OUTPUT - opt_pars - 3D matrix of optimized pars, with dimensions:
% - number of optimized pars
% - number of repetitions
% - number of perturbations (1 for no pert)
% - min_pars - 3D matrix of lower bounds;
% same dimesions as opt_pars
% - max_pars - 3D matrix of upper bounds;
% same dimesions as opt_pars
% - whitelist - structure where fields are names of 'groups', each
% field is 4D array with 1/0
% - dh_pars - structure where fields are names of 'groups', each
% field is 4D array with kinematics parameters
% Copyright (C) 2019-2021 Jakub Rozlivek and Lukas Rustler
% Department of Cybernetics, Faculty of Electrical Engineering,
% Czech Technical University in Prague
%
% This file is part of Multisensorial robot calibration toolbox (MRC).
%
% MRC is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MRC is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MRC. If not, see <http://www.gnu.org/licenses/>.
%% Call appropriate function or select default
if(nargin == 8)
if ischar(funcname) || isstring(funcname)
func=str2func(funcname);
whitelist = func();
else
whitelist = funcname;
end
else
whitelist = robot.structure.WL;
end
fnames = fieldnames(robot.structure.kinematics);
WLfnames = fieldnames(whitelist);
for fname=WLfnames'
if ~any(ismember(fnames, fname{1}))
whitelist = rmfield(whitelist, fname{1});
end
end
for fname=fnames'
if ~any(ismember(WLfnames, fname{1}))
robot.structure.WL.(fname{1}) = zeros(size(robot.structure.kinematics.(fname{1})));
end
end
whitelist = group.sort(whitelist);
[whitelist, ~] = padVectors(whitelist, 1);
%Edit each links
for link=robot.links
link=link{1};
if ~strcmp(link.type,'base')
% if calibrate only offsets - set all other params to zeros
if linkTypes.onlyOffsets && isfield(whitelist, link.group)
whitelist.(link.group)(link.DHindex,1:5)=0;
end
% link is in non-calibrated 'type' or non-calibrated 'group' set
% whitelist to zeros
if (~linkTypes.(link.type) || ~chains.(strrep(link.group,'Skin',''))) && isfield(whitelist, link.group)
whitelist.(link.group)(link.DHindex,:)=zeros(1,6);
end
end
end
%% Allocate matrices
names = fieldnames(whitelist);
count = 0;
for index = 1:size(names,1)
count = count + sum(sum(whitelist.(names{index})));
end
opt_pars = inf(count, optim.repetitions, optim.pert_levels);
max_pars = inf(count, optim.repetitions, optim.pert_levels);
min_pars = inf(count, optim.repetitions, optim.pert_levels);
%%
index = 1;
for name = 1:size(names,1)
whitelist.(names{name}) = logical(whitelist.(names{name}));
% get whitelist in right format
b = whitelist.(names{name})';
for pert = 1:(optim.pert_levels)
for rep = 1:optim.repetitions
% get params on given indexes
a = dh_pars.(names{name})(:,:,rep,pert)';
% get kinematics withou perturabtions
c=dh_pars.(names{name})(:,:,1,1)';
% replace non-optimized values with non-perturbed ones
a(~b)=c(~b);
% copy back to dh_pars
dh_pars.(names{name})(:,:,rep,pert)=a';
lb = lb_pars.(names{name})(:,:,rep,pert)';
ub = ub_pars.(names{name})(:,:,rep,pert)';
new_index = index + length(a(b))-1;
% Append to output variables
opt_pars(index:new_index,rep,pert) = a(b);
min_pars(index:new_index,rep,pert) = lb(b);
max_pars(index:new_index,rep,pert) = ub(b);
end
end
index = new_index+1;
end
end