VSL - Visuospatial Skill Learning (Reproduction)
This source code is the implementation of the algorithms described in Chapter 4 of the book “Handling Uncertainty and Networked Structure in Robot Control”, Lucian Busoniu and Levente Tamas (eds.), Springer, 2015.
Author: Reza Ahmadzadeh, 2015
% http://www.ahmadzadeh.info % % The program consists of two phases: demonstration and reproduction. % in the demonstration phase, it loads 3 captured images including a % pre-action, a post-action, and another pre-action from the next action. % The VSL experiment includes (described in the chapter) a pick-and-place % operation in which the tutor picks and places each object in the scene. % The robot observes the operation and capture images. Two observations are % extracted from the images (a pre-action and a post-action observation). % The pre-action observation represents the manipulated object and the % post-action observation represent the place that the object was moved to. % In the demonstration phase, the robot learns the spatial relationship % between objects and their surrounding environment. % % In the reproduction phase the objects are randomized in the scene. Still % the robot can detect the objects and execute a sequence of operations to % reach the goal of the task. % % This source code is given for free! However, I would be grateful if you refer % to the book (or corresponding article) in any academic publication that uses % this code or part of it. Here are the corresponding BibTex references: % % % @inproceedings{ahmadzadeh2013interactive, % title={Interactive Robot Learning of Visuospatial Skills}, % author={Ahmadzadeh, Seyed Reza and Kormushev, Petar and Caldwell, Darwin. G.}, % booktitle={Advanced Robotics (ICAR) 2013, 16th International Conference on}, % pages={1--8}, % year={2013}, % organization={IEEE} % } % % @inproceedings{ahmadzadeh2013visuospatial, % title={Visuospatial Skill Learning for Object Reconfiguration Tasks}, % author={Ahmadzadeh, Seyed Reza and Kormushev, Petar and Caldwell, Darwin G.}, % booktitle={Intelligent Robots and Systems ({IROS}), 2013 {IEEE/RSJ} International Conference on}, % pages={685--691}, % year={2013}, % organization={IEEE} % } % % % This is a simple demo that represents the implementation of the VSL % algorithm. This also can be considered as a validation of the concept. % % ** This script includes the reproduction phase of the algorithm. You have to % run the demonstration script before running this script. % % ================================================ % Reza Ahmadzadeh (reza.ahmadzadeh@iit.it) % ================================================ clc,% clear, close all; fprintf('VSL - Visuospatial Skill Learning\n'); fprintf('---------------------------------------\n'); fprintf('--- Reproduction ---\n'); fprintf('---------------------------------------\n'); fprintf('For this Demo, we consider that a simple pick-and-place \n'); fprintf('action was demonstrated on a set of objects and the learning \n'); fprintf('process is finished. Now the objects are reshuffled randomly \n'); fprintf('in the world and the robot is excpected to be able to reproduce \n'); fprintf('the demonstrated skill. \n'); % ---------------------------------------------------- % Capturing images from the camera...\n'); % ---------------------------------------------------- % in this demo we are not capturing from camera. Use this section to % capture from the camera. % ---------------------------------------------------- % Reading and drawing the raw images (reproduction phase) % ---------------------------------------------------- preAction1r = imread('img\reproduction\preActionObservation.png'); I1r = im2double(preAction1r); % [r,c,~] = size(I1r); fprintf('The images are rectified using the homography matrix.\n'); % show the raw images figure;set(gcf,'position',get(0,'ScreenSize')); imshow(preAction1r);title('pre-action observation'); fprintf('\n The image shows the new world in which the objects are randomly replaced.\n'); fprintf('To continue press Enter');pause; clear preAction1r % ---------------------------------------------------- % finding the the first object to manipulate using the % previus learned skills (recorded observations). % ---------------------------------------------------- fprintf('\n\n------\nSTEP-1\n------\n'); fprintf('finding the object to be manipulated using the \n learned skill and the recorded observations\n'); % ----> convert the rgb images to grayscale I1rg = rgb2gray(I1r); obj1g = rgb2gray(obj1); place1g = rgb2gray(place1); fprintf('Here we extract SURF features for the observations from the demonstration phase \n to detect the object in the scence.\n'); fprintf('Other metrics and features such as SIFT also can be used.\n'); % ----> detecting SURF features (other metrics such as SIFT also can be used.) % scenePoints includes the SURF features for the object scenePoints = detectSURFFeatures(I1rg,'MetricThreshold',500); objPoints = detectSURFFeatures(obj1g,'MetricThreshold',500); placePoints = detectSURFFeatures(place1g,'MetricThreshold',700); fprintf('\nextracting SURF features ...'); % ----> extract feature descriptors [sceneFeatures, scenePoints] = extractFeatures(I1rg, scenePoints); [objFeatures, objPoints] = extractFeatures(obj1g, objPoints); [placeFeatures, placePoints] = extractFeatures(place1g, placePoints); % ----> object pair (between the object and the scene) objPairs = matchFeatures(objFeatures, sceneFeatures); fprintf('\nfeatures are extracted.'); fprintf('\n\n------\nSTEP-2\n------\n'); fprintf('Match finding between the demonstrated observations and \n and the current world based on the learned skills\n'); fprintf('match finding in progress....'); % ----> match finding matchedObjPoints = objPoints(objPairs(:, 1), :); matchedScenePoints = scenePoints(objPairs(:, 2), :); % figure; % showMatchedFeatures(obj1g, I1rg, matchedObjPoints,matchedScenePoints, 'montage'); % title('Putatively Matched Points (Including Outliers)'); [tform, inlierBoxPoints, inlierScenePoints] = estimateGeometricTransform(matchedObjPoints, matchedScenePoints, 'affine'); % ----> show the match features figure;set(gcf,'position',get(0,'ScreenSize')); subplot(2,2,1); showMatchedFeatures(obj1g, I1rg, matchedObjPoints,matchedScenePoints, 'montage'); title('Matched Points (Inliers Only)'); objPolygon = [1, 1;... % top-left size(obj1, 2), 1;... % top-right size(obj1, 2), size(obj1, 1);... % bottom-right 1, size(obj1, 1);... % bottom-left 1, 1]; % top-left again to close the polygon newObjPolygon = transformPointsForward(tform, objPolygon); TT = tform.T; cxx1 = fix(TT(3,1) + size(obj1g,1)/2); cyy1 = fix(TT(3,2) + size(obj1g,1)/2); subplot(2,2,3);imshow(I1r);hold on; line(newObjPolygon(:, 1), newObjPolygon(:, 2), 'Color', 'y'); title('Detected Object in the new scene'); hold on;plot(cxx1,cyy1,'+r'); clear tform inlierBoxPoints inlierScenePoints objPolygon TT fprintf('To continue press Enter');pause; % ----> place pair (between the place and the scene) placePairs = matchFeatures(placeFeatures, sceneFeatures); % ----> match finding matchedPlacePoints = placePoints(placePairs(:, 1), :); matchedScenePoints = scenePoints(placePairs(:, 2), :); [tform, inlierBoxPoints, inlierScenePoints] = estimateGeometricTransform(matchedPlacePoints, matchedScenePoints, 'affine'); fprintf('\nmatch finding finished.'); % ----> show the match features subplot(2,2,2); showMatchedFeatures(place1g, I1rg, matchedPlacePoints,matchedScenePoints, 'montage'); title('Matched Points (Inliers Only)'); placePolygon = [1, 1;... % top-left size(place1g, 2), 1;... % top-right size(place1g, 2), size(place1g, 1);... % bottom-right 1, size(place1g, 1);... % bottom-left 1, 1]; % top-left again to close the polygon newPlacePolygon = transformPointsForward(tform, placePolygon); TT = tform.T; cxx2 = fix(TT(3,1) + size(place1g,1)/2); cyy2 = fix(TT(3,2) + size(place1g,1)/2); subplot(2,2,4);imshow(I1r);hold on; line(newPlacePolygon(:, 1), newPlacePolygon(:, 2), 'Color', 'y'); title('Detected Object in the new scene'); hold on;plot(cxx2,cyy2,'+r'); fprintf('The pick and place points are generated by the algorithm. \n'); fprintf('To continue press Enter');pause; clear scenePoints objPoints placePoints clear sceneFeatures objFeatures placeFeatures clear objPairs matchedObjPoints matchedScenePoints clear placePairs matchedPlacePoints matchedScenePoints clear tform inlierBoxPoints inlierScenePoints clear placePolygon TT fprintf('\nThe final operation that the robot has to perform.'); fprintf('\n**Note that the drawn polygon illustrate an area of the object'); fprintf('\nbased on the detected features. They are plotted just for illustration purposed.' ); figure; imshow(I1r);hold on; line(newObjPolygon(:, 1), newObjPolygon(:, 2), 'Color', 'y');hold on;plot(cxx1,cyy1,'+r'); line(newPlacePolygon(:, 1), newPlacePolygon(:, 2), 'Color', 'g');hold on;plot(cxx2,cyy2,'+r'); hold on;quiver( cxx1,cyy1,cxx2-cxx1,cyy2-cyy1,0 ) ; title('reproduction of the learned operation'); xlabel('the blue arrow shows the action that the robot should take.'); fprintf('\nThe code is a simple implementation of VSL and shows a single pick-and-place operation.'); fprintf('\n To perform more operation, you should check the corresponding chapter and related articles.'); fprintf('\n\n------\nSTEP-3\n------\n'); fprintf('\nNow by passing the coordinates of the pick-point and the place-point'); fprintf('\nto a trajectory generation module, a trajectory can be generated and executed'); fprintf('\non the robot. For more information please refer to the chapter and corresponding articles.');
VSL - Visuospatial Skill Learning --------------------------------------- --- Reproduction --- --------------------------------------- For this Demo, we consider that a simple pick-and-place action was demonstrated on a set of objects and the learning process is finished. Now the objects are reshuffled randomly in the world and the robot is excpected to be able to reproduce the demonstrated skill. The images are rectified using the homography matrix. Warning: Image is too big to fit on screen; displaying at 50% The image shows the new world in which the objects are randomly replaced. To continue press Enter ------ STEP-1 ------ finding the object to be manipulated using the learned skill and the recorded observations Here we extract SURF features for the observations from the demonstration phase to detect the object in the scence. Other metrics and features such as SIFT also can be used. extracting SURF features ... features are extracted. ------ STEP-2 ------ Match finding between the demonstrated observations and and the current world based on the learned skills match finding in progress....To continue press Enter match finding finished.The pick and place points are generated by the algorithm. To continue press Enter The final operation that the robot has to perform. **Note that the drawn polygon illustrate an area of the object based on the detected features. They are plotted just for illustration purposed.Warning: Image is too big to fit on screen; displaying at 50% The code is a simple implementation of VSL and shows a single pick-and-place operation. To perform more operation, you should check the corresponding chapter and related articles. ------ STEP-3 ------ Now by passing the coordinates of the pick-point and the place-point to a trajectory generation module, a trajectory can be generated and executed on the robot. For more information please refer to the chapter and corresponding articles.
