diff --git a/@Robot/getResultKinematics.m b/@Robot/getResultKinematics.m index 931cc26..3cd0e4e 100755 --- a/@Robot/getResultKinematics.m +++ b/@Robot/getResultKinematics.m @@ -61,6 +61,9 @@ results.(fnames{field})(:,4:6,:,:)=ezwraptopi(results.(fnames{field})(:,4:6,:,:)); count = new_count; end + if (optim.skipNoPert) + results.(fnames{field})(:,:,:,1) = start_dh.(fnames{field})(:,:,:,1); + end mResults = results.(fnames{field}); % corrections = results - default corrs.(fnames{field}) = zeros(size(mResults)); @@ -69,6 +72,9 @@ % wrap to [-pi,pi] corrs.(fnames{field})(:,4:6,:,:)=ezwraptopi(corrs.(fnames{field})(:,4:6,:,:)); start_dh.(fnames{field})(:,1:3,:,:) = start_dh.(fnames{field})(:,1:3,:,:)/optim.unitsCoef; + if (optim.skipNoPert) + corrs.(fnames{field})(:,:,:,1) = nan; + end end end diff --git a/Calib/getDist.m b/Calib/getDist.m index dede935..39b1009 100755 --- a/Calib/getDist.m +++ b/Calib/getDist.m @@ -36,12 +36,11 @@ for dataset=datasets dataset = dataset{1}; refPoints = dataset.refPoints; - computeArm2 = ~optim.refPoints || (isempty(refPoints)); % compute RT matrices and transform points to base frame - [arm1,arm2] = getPointsIntern(dh_pars, dataset, computeArm2, robot.structure.type); - - % if only one arm, use the refPoints - if(~computeArm2) + if (~optim.refPoints || (isempty(refPoints))) + [arm1,arm2] = getPointsIntern(dh_pars, dataset, robot.structure.type); + else % if only one arm, use the refPoints + arm1 = getPointsIntern(dh_pars, dataset, robot.structure.type); arm2 = refPoints'; end diff --git a/Calib/getDistFromExt.m b/Calib/getDistFromExt.m index 386e0c9..f0f0946 100755 --- a/Calib/getDistFromExt.m +++ b/Calib/getDistFromExt.m @@ -37,7 +37,7 @@ dataset = datasets{datasetId}; extPoints=dataset.refPoints; % compute points in the base frame - robPoints = getPointsIntern(dh_pars, dataset, false, robot.structure.type); + robPoints = getPointsIntern(dh_pars, dataset, robot.structure.type); if isempty(extParams) % find transformation between external camera and robot [R,T]=fitSets(extPoints,robPoints(1:3,:)'); diff --git a/Calib/getPlaneDist.m b/Calib/getPlaneDist.m index cfbb0ee..1f50291 100755 --- a/Calib/getPlaneDist.m +++ b/Calib/getPlaneDist.m @@ -31,10 +31,10 @@ dist = []; - for datasetId=length(datasets) + for datasetId=1:length(datasets) dataset = datasets{datasetId}; % compute points in the base frame - robPoints = getPointsIntern(dh_pars, dataset, false, robot.structure.type); + robPoints = getPointsIntern(dh_pars, dataset, robot.structure.type); if isempty(planePars) % compute the plane plane = getPlane(robPoints); diff --git a/Calib/getProjectionDist.m b/Calib/getProjectionDist.m index 9bed38d..7e01b5a 100755 --- a/Calib/getProjectionDist.m +++ b/Calib/getProjectionDist.m @@ -32,7 +32,7 @@ dist = []; coeffs = []; - cam_frames = robot.findJointByType('eye'); + cam_frames = robot.findLinkByType('eye'); type = robot.structure.type; for dataset=datasets dataset = dataset{1}; diff --git a/README.md b/README.md index e5a9b6d..3548d46 100755 --- a/README.md +++ b/README.md @@ -1,7 +1,11 @@ # Multisensorial robot calibration framework and toolbox This toolbox provides a solution to the multi-chain calibration of a general robot by combining multiple calibration approaches. Users can define an arbitrary robot, choose calibration approach, set parameters of the optimization solver and the calibration. The results can be saved for later use and evaluated with a variety of prepared visualizations. Also, a user-friendly GUI is available, which makes the calibration of robots accessible even for the general public. -![pipeline](overview.png) +![pipeline](overview.png) + +**Check out youtube video as well** +[![Youtube video](yt_thumb.png)](https://www.youtube.com/watch?v=ZZHztHF6eNs) + # Contents @@ -23,7 +27,7 @@ This toolbox provides a solution to the multi-chain calibration of a general rob # How to run Recommended steps (the order is optional): - - [Download the GUI installation file and user data](https://github.com/ctu-vras/multirobot-calibration/releases/latest) (Configuration file pattern, Robot function pattern, Example codes, csv file pattern) - **always download the lastest release!** + - [Download the GUI installation file and user data](https://gitlab.fel.cvut.cz/body-schema/code-calib-multirobot/-/releases) (Configuration file pattern, Robot function pattern, Example codes, csv file pattern) - **always download the lastest release!** - Install the GUI in Matlab by double clicking on the file. The toolbox directory (depending on your Matlab Preferences - Add-ons - Installation Folder) will be added to the Matlab "path". - Unpack the additionalFiles.zip into a folder of your choice. This will give you access to the folders discussed below. - **Recommendation:** Create a working folder, move the additional files there and set it as Current Folder in Matlab. diff --git a/UserData/Robots/Motoman/loadNewLeicaDataset.m b/UserData/Robots/Motoman/loadNewLeicaDataset.m index 379c762..ea119c2 100755 --- a/UserData/Robots/Motoman/loadNewLeicaDataset.m +++ b/UserData/Robots/Motoman/loadNewLeicaDataset.m @@ -11,7 +11,7 @@ % OUTPUT - datasets - 1x4 ([self-touch, planes, external, projection]) % cell array of cell arrays (1xN) of datasets - assert(~isempty(rob.findJoint('LR1'))) + assert(~isempty(rob.findLink('LR1'))) dataset.cameras = []; diff --git a/Utils/CalibUtils/initialGuess.m b/Utils/CalibUtils/initialGuess.m index 787a7d9..2ece4fe 100755 --- a/Utils/CalibUtils/initialGuess.m +++ b/Utils/CalibUtils/initialGuess.m @@ -1,4 +1,4 @@ -function [params, typicalX] = initialGuess(datasets, dh_pars, approaches, optim) +function [params, typicalX] = initialGuess(datasets, dh_pars, approaches, optim, dh_type) %INITIALGUESS Compute initial guess of plane and/or external transformation parameters % Compute plane parameters for each dataset.planes and rotation vector % (quaternion) and translation vector for each dataset.external separately @@ -39,7 +39,7 @@ if approaches.(type) for dataset = datasets.(type) dataset = dataset{1}; - robPoints = getPointsIntern(dh_pars, dataset, false); + robPoints = getPointsIntern(dh_pars, dataset, dh_type); if strcmp(type,'planes') newParams = getPlane(robPoints); newParams = newParams(1:3)/newParams(4); % plane equation in format ax+by+cz = 1 diff --git a/Utils/CalibUtils/internalCalibration.m b/Utils/CalibUtils/internalCalibration.m index 720d0b7..c4e7e69 100644 --- a/Utils/CalibUtils/internalCalibration.m +++ b/Utils/CalibUtils/internalCalibration.m @@ -85,7 +85,7 @@ function internalCalibration(rob, config, whpars, data, folder, saveInfo, saveDa end initialParamValues = []; if optim.optimizeInitialGuess && (approach.planes || approach.external) - [params, typicalX]=initialGuess(tr_datasets, kinematics, approach, optim); + [params, typicalX]=initialGuess(tr_datasets, kinematics, approach, optim, robot.structure.type); options.TypicalX(end-length(params)+1:end) = typicalX; if(optim.optimizeDifferences) initialParamValues = params; diff --git a/Utils/CalibUtils/loadTasksFromFile.m b/Utils/CalibUtils/loadTasksFromFile.m index f70f698..901a9eb 100755 --- a/Utils/CalibUtils/loadTasksFromFile.m +++ b/Utils/CalibUtils/loadTasksFromFile.m @@ -1,7 +1,8 @@ -function loadTasksFromFile(fileName) +function loadTasksFromFile(fileName, startRow) %LOADTASKSFROMFILE Loading the calibration task from csv file %Function for loading calibrations tasks from csv file and running main %INPUT - fileName - csv file name + % - startRow - index of first row (default 1) % Copyright (C) 2019-2021 Jakub Rozlivek and Lukas Rustler @@ -23,6 +24,9 @@ function loadTasksFromFile(fileName) % You should have received a copy of the GNU Leser General Public License % along with MRC. If not, see . + if (nargin < 2) + startRow = 1; + end %% loading column names and type from file opts=detectImportOptions(fileName); @@ -34,7 +38,7 @@ function loadTasksFromFile(fileName) file=readtable(fileName,opts); keyWords={'min', 'max', 'median'}; % type of selecting kinematics from Mat - for lineId=1:size(file,1) + for lineId=startRow:size(file,1) line=table2cell(file(lineId,:)); %% save info switch if strcmpi(line{11},'true') diff --git a/Utils/KinUtils/getPoints.m b/Utils/KinUtils/getPoints.m index 4a39f91..aa71951 100644 --- a/Utils/KinUtils/getPoints.m +++ b/Utils/KinUtils/getPoints.m @@ -41,16 +41,16 @@ end if(~isempty(joints)) for i = 1:size(joints, 1) - if (isobject(frames{i})) - arm1(:,i) = getTFtoFrame(dh_pars,frames{i},joints(i)) *[points(i,1:3),1]'; + if (isobject(frames(i))) + arm1(:,i) = getTFtoFrame(dh_pars,frames(i),joints(i)) *[points(i,1:3),1]'; else f=robot.findLink(frames{i}); arm1(:,i) = getTFtoFrame(dh_pars,f{1},joints(i)) *[points(i,1:3),1]'; end if compute_arm2 - if (isobject(frames2{i})) - arm2(:,i) = getTFtoFrame(dh_pars,frames2{i}, joints(i)) *[points(i,4:6),1]'; + if (isobject(frames2(i))) + arm2(:,i) = getTFtoFrame(dh_pars,frames2(i), joints(i)) *[points(i,4:6),1]'; else f=robot.findLink(frames2{i}); arm2(:,i) = getTFtoFrame(dh_pars,f{1}, joints(i)) *[points(i,4:6),1]'; diff --git a/Utils/KinUtils/getPointsIntern.m b/Utils/KinUtils/getPointsIntern.m index 240c454..28414cf 100755 --- a/Utils/KinUtils/getPointsIntern.m +++ b/Utils/KinUtils/getPointsIntern.m @@ -1,10 +1,8 @@ -function [ arm1, arm2 ] = getPointsIntern(dh_pars, dataset, compute_arm2, type) +function [ arm1, arm2 ] = getPointsIntern(dh_pars, dataset, type) %GETPOINTSINTERN Compute points coordinates to base. %INPUT - dh_pars - structure with kinematics parameters, where field names corresponding to names of % the 'groups' in robot. Each group is matrix. % - dataset - dataset structure in common format - % - compute_arm2 - whether compute second end effector or use - % refPoints %OUTPUT - arm1 - points coordinates of first end effector % - arm2 - points coordinates of second end effector @@ -33,6 +31,7 @@ joints = dataset.joints; points = dataset.point; arm1 = zeros(4, size(joints, 1)); + compute_arm2 = nargout == 2; if(compute_arm2) arm2 = zeros(4, size(joints, 1)); frames2 = dataset.frame2; diff --git a/Visualisation/plotCorrections.m b/Visualisation/plotCorrections.m index ca2b80a..99984fa 100755 --- a/Visualisation/plotCorrections.m +++ b/Visualisation/plotCorrections.m @@ -62,7 +62,7 @@ function plotCorrections(folder, varargin) units = info.optim.units; end - fnames=fieldnames(corrections); + fnames=fieldnames(whitelist); for name = 1:size(fnames,1) whitelist.(fnames{name}) = double(whitelist.(fnames{name})); diff --git a/yt_thumb.png b/yt_thumb.png new file mode 100644 index 0000000..2d78197 Binary files /dev/null and b/yt_thumb.png differ